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ABSTRACT 


The Navy’s leadership is looking at improving quality of life and reducing long 
term health problems through the reduction of knee disorders. One proposal for reducing 
knee disorders is to install more compliant decking. The goal of this thesis is to develop a 
computer model of the human gait that estimates the transarticulation forces in the knee 
during walking on various surfaces. This model can be used to evaluate the reduction of 
the heel strike forces during walking when deck surface modifications are made. 
Previous analytical and computer models of the human gait are reviewed. The major 
contribution of this thesis is a detailed dynamic model of foot-ground interaction during 
the initial phase of load bearing in human gait. 
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I. INTRODUCTION 


A. GOALS 

The goal of this thesis is to develop a two-dimensional computer simulation 
model of a human that has the ability to interact with and simulate mechanical properties 
of different floor materials. The main intent is to provide a means to determine whether 
or not a softer or more compliant floor material will alter the characteristics of the initial 
contact forces during human gait and, therefore, the transarticulation forces within the 
knee of a human during walking. This thesis discusses the relationship between ground 
reaction impulsive-forces and knee disorders, namely chrondomalacia and osteoarthritis, 
and its relevance to the United States Navy. The two-dimensional mathematical model 
used is a four-mass sagittal plane linkage model with hard constraints in the joints and 
explicit representation of floor compliance. 

This work arises from the desire to improve the quality of life of sailors during 
and after life on board ships and to reduce the costs associated with lost training, short¬ 
term medical care and long-term disability payments. This thesis makes a connection 
between the hard steel decks of ships, the impulse force felt by the human body during 
gait, and the above named knee disorders. 

B. ORGANIZATION 

Chapter n of this thesis outlines the background and motivation, including a look 
at the knee problems associated with shipboard life and effects of the impulse forces 
during the initial impact phase of a human gait. Chapter m reviews previous models for 
computer simulation of the dynamics of human motion, including a postural control 
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model from which this thesis model was developed. Chapter IV provides an overview 
of kinematic and dynamic modeling of articulated rigid bodies. The analysis includes 
“inverse” and “direct” dynamic problems and methods to solve each. 

Chapter V describes the four-link human mathematical model used to calculate 
the results. Chapter VI presents the results of the data obtained from the model. Ch^ter 
Vn summarizes and draws conclusions from the results, offers suggestions for applying 
these results to shipboard life, and recommends improvements to the model and further 
research. 
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n. BACKGROUND AND MOTIVATION 

A. BACKGROUND 

1. Knee Disorders in the U.S. Navy 

Orthopedic knee disorders account for three of the top 10 medical reasons U. S. 
Naval personnel separate from active duty service (DeMaio 1999). These knee disorders 
need to be reduced in order to improve the quality of life, reduce the pain and costs 
associated with long-term health problems, and reduce the cost of replacing skilled 
workers. One common injury particularly associated with shipboard life is 
patellofemoral chondrosis (DeMaio 1999). 

Between 1979 and 1981, Helmkamp and Coben (1985) smdied the U. S. Navy 
and Marine Corps enlisted males who entered service in calendar year 1974. Of those 
entrants, 989 suffered a knee injury that resulted in hospitalization. Medical Boards, 
and/or Physical Evaluation Boards. Helmkamp and Coben listed the knee problems as 
loose bodies in the knee. Chondromalacia, other knee derangement, other knee disorders, 
fractured patella, and dislocated knee. Their study showed that Chondromalacia ranked 
number one for medical boards and physical evaluation boards for males with a knee 
diagnosis in this group. 

2. Medical Definitions 

Hoppenfeld and Zeide (1994) define Chondromalacia as "a pathologic state of 
softening with subsequent fibrillation (a structural defect seen in articular cartilage that is 
characterized by splitting of the cartilage surface into fibril-like projections), fissuring (a 
deep, linear groove, ulcer, or crack with sharp edges), and erosion of articular cartilage." 
Articular cartilage is a specialized type of smooth and homogeneous cartilage that 
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provides the self-lubricating, low-friction gliding and load-distributing surface of 
synovial joints. Pathoanatomic knee changes may be classified into the following four 
groups (Hoppenfeld and Zeide 1994): 

Group I: There is swelling and softening of the articular cartilage but the surface 
remains intact. Also known as Closed Chondromalacia. 

Group II: There is fissuiing in the softened areas. 

Group ni: There is fibrillation or fasciculation of the articular cartilage. 

Group rV: There is complete erosion of articular cartilage, with exposed 
subchondral bone. 

Chondromalacia Patella is defined as "pathological changes in the articular 
cartilage of the undersurface of the patella (knee cap), ainically, this may be 
associated with a syndrome of anterior knee pain in which the patient complains 
of persistent pain behind the patella, especially after sitting with knees flexed for a 
period of time or when the knee is loaded in the flexed position. The degree of 
clinical symptoms does not always correlate well with the extent of pathological 
changes." (Hoppenfeld and Zeide 1994). 

3. Biomechanics of Gait>Initial Contact 

In studies by orthopedic surgeons, the human stride has been broken down into 
eight functional patterns or phases (Perry 1992). The first phase is called “initial impact”, 
also referred to as “heel strike” in the case of a non-pathological gait. Classically, it has 
been the point at which the analysis of the gait cycle or stride and specifically the stance 
period begins, and it lasts for approximately 2% of the gait cycle. The gait is also divided 
into tasks, where the initial impact is found in the weight acceptance task of the cycle. 
The loading of the limb and subsequently the joints are determined by the posture at the 
time of initial impact. 
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Analysis of the first few milliseconds of gait just before the initial impact shows 
an unbalanced situation where the center of gravity of the body and limbs are forward of 
hind leg, which has the only contact with the ground. This point becomes a pivot, and the 
body is temporarily in a state of free-fall. Jacquelin Perry states that this free-fall occurs 
as the foot that is about to take the impact is approximately 1 cm above the ground (Perry 
1992). She also states that 60% of the body weight is loaded on the forward limb in 0.02 
seconds. 

Various papers quantify this initial impact force in several different ways. Many 
early force plate measurements do not show this impulse force (Bresler and Frankel 1950, 
Siegler et al 1982, Chao et al. 1983, and Collins and Whittle 1989b). The absence of the 
peak force can be attributed to one or more of the following conditions: the sampling rate 
and/or characteristics of the force plate (Simon et al 1981 and Collins and Whittle 
1989b), averaging of the data (Chao et al. 1983), not beginning data collection imtil the 
transient has began, or the force not actually being present in the subjects gait or 
attenuation due to shoes (Cavanagh et al. 1981). The magnitude of the initial impact 
impulse force varies from none to a peak of 1.25 times body force (Simon et al. 1981). A 
higher data sampling rate and characteristics of the force plate seem to have the greatest 
effect on the accurate measurement of the peak force. 

All sources agree on the shape of the vertical ground reaction force that follows. 
It is the typical double hump. The first hump is associated with loading of the heel, a 
drop follows due to mid-stance knee flexion, and another hump as weight is shifted to the 
forefoot and push-off sets up the next heel strike. Figure 2.1 shows a typical vertical 
ground reaction force graph (Collins and Whittle 1989a). 
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Figure 2.1. Typical plot of ground reaction forces versus time. Modified from 
Collins and Whittle 1989a. 

Simon et al. (1981) conducted an extensive study of the initial impact impulse 
force. He calls it the “peak dynamic force.’* He used a Kistler quartz multi-component 
measuring platform and found the peak magnitude to vary between 0.5 to 1.25 times 
body weight. The peak was much higher when the subject walked on concrete versus 
wood. 


In order to attenuate the vertical ground reaction forces, the body undergoes 
several ingenious shock absorbing processes. The first response is “free ankle plantar 
flexion” immediately following heel impact (Perry 1992). The ankle falls quickly for 10° 
before the pretibial muscle slows the fall and transfers the motion and momentum to the 
shank (or tibia or leg). The final two shock absorbing mechanisms are knee flexion and 
muscle action in the hip during a contralateral pelvic drop (Perry 1992). The body 
appears to have no natural attenuation for the initial impact impulse force. It is a function 
of the characteristics of the ground, shoe, heel pad, and gait parameters (velocity, stride 
length, and angle of foot with respect to ground) (Simon et al. 1981). 
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4. Effects of Impulse Forces During Walking 

Simon et al. (1981) makes the connection between high impulse heel strike 
transients and osteoarthritis. Collins and Whittle (1989a) review nearly a hundred papers 
relating to the clinical implications of the impulse force. They show that lower back pain 
and osteoarthrosis are directly related to the repetitive impulse forces encountered by the 
human musculo-skeletal system. Although they state that several differences of opinion 
exist among the experts as to what is actually going on in vivo (within the living 
organism) as to cause the degradation, most experts do agree that the impulse force is a 
significant contributing factor to whatever may be happening inside the body. 

Two methods are given to reduce the initial impact impulse force during walking. 

1. Softer surfaces 

2. Orthotic inserts. 

For the first method, they cite five references that compare soft and hard surfaces. The 
first reference reported that pigs housed in buildings with hard surfaces were more likely 
to develop osteoarthrosis than pigs housed in building with dirt floors (Ross 1968). Next, 
they reference Simon et al. (1981) who show that a harder surface increases the 
frequencies and magnitudes of the ground reaction forces. The last three references are 
to E. L. Radin, who investigated the effects of sheep walking on concrete as compared to 
walking on wood chips. He concluded, “significant changes occur in both cartilage and 
bone as a result of prolonged walking on hard surfaces” (Radin 1982). 

For the second method of reducing impulse forces during walking, Collins and 
Whittle (1989a) show that orthotic inserts reduced the maximum amplitudes of the bone 
acceleration by citing Voloshin (1981) and Wosk (1985). Voloshin (1981) shows that 
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artificial shock absorbers in the form of viscoelastic arch support inserts attenuate by 
42% the amplitude of the incoming shock waves from heel strike. Wosk (1985) uses 
these artificial shock absorbers to reduce lower back pain associated with walking. 

Guilak et al. (1997) point out that studies have been done that connect repetitive 
impact loading as related to repetitious daily activity with mechanical changes to the 
cartilage. These impact loadings have been shown to cause immediate and progressive 
damage to the articular cartilage. This articular damage, cited by both Collins and 
Guilak, is the onset of osteoarthrosis and chondromalacia as described by Hoppenfeld 
and Zeide. 

B. MOTIVATION 

Obviously, if the impulse forces associated with initial ground impact during 
human gait are reduced, the medical separations, lowered quality of life, cost of 
retraining, and long-term cost of disability will also be reduced. Compliant surfaces have 
been shown to reduce the initial impact impulse force peak. The Department of the Navy 
has military specifications (Milspecs) thoroughly outlining the requirements as to the 
quality, durability, and wear, among other things, of acceptable decks and deck coverings 
for surface ships (S9086-VG-STM-010/CH-634). The question asked is “How does one 
quantitatively evaluate the reduction of this peak through the adding of more compliant 
surfaces, such as athletic track material, to the decks of ships in order to reduce the initial 
impact loading and ground reaction forces in the human gait while at the same time 
meeting the milspec requirements?" 

The quantitative effect of mechanical properties of the surface material on the 
initial impact impulse force associated with human gait can be determined through 
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experimental measurement or analytical modeling. Three experimental methods include 
force plate measurements, in shoe force measurements, and limb acceleration 
measurements. Chapter m of this thesis surveys papers that use experimental or 
analytical methods in evaluating human gait. 

This thesis modifies an existing model of a human gait by adding an explicit foot 
dynamic model and a spring damper system for the more compliant deck, then lays the 
groundwork for a quantitative comparison of the magnitude of the initial impact impulse 
force for different surfaces. This thesis also reviews other models for their potential 
modification and application to this and other gait related problems. 

C. SUMMARY 

Chondromalacia, the leading cause of knee problems sent to Medical Boards and 
Physical Evaluation Boards in the U. S. Navy has been defined. Studies have tied patella 
chondromalacia and osteoarthritis as well as back pain to the initial impact impulse force 
during walking. This force’s peak is greatest on hard surfaces similar to steel decks on 
ships. Attenuating the initial impact impulse force by means of a softer or more 
compliant deck covering needs to be quantified. Various authors have made strides in the 
area of studying a human gait, which have contributed to the quantification methodology 
established in this thesis. 
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in. SURVEY OF PREVIOUS WORK 


A. INTRODUCTION 

Initial impact impulse forces of human gait must be measured or calculated in 
order to better understand and eventually quantify the effects of material properties, 
namely, compliance, of a surface on the impulse force. Human gait has been studied 
extensively, especially in the area of measuring ground reaction forces. Usually these 
forces and the motion of the body are used to determine joint forces and moments using 
inverse dynamics models. Additionally, ground reaction forces and center of pressure 
were measured and used to aid diagnosis of pathological gait. This thesis looks at 
measuring the ground reaction forces in order to capture the initial impact impulse force. 

This chapter reviews a qualitative study of deck surfaces, shoes, and the effects on 
the body, established methods to measure ground reaction forces, and briefly looks at 
analytical models used to calculate the ground reaction forces. Additionally, it reviews 
the postural control model used as the basis for this thesis. 

B. QUALITATIVE STUDY OF SOFT SHOES AND SOFT MATS 

Hansen et al. (1998), conducted an interesting study in which four combinations 
of soft shoes, clogs, soft mat and concrete were compared during two hours of standing 
and two hours of standing/walking work. This type of work appears to closely resemble 
the watchstanding routine for most personnel on the bridges of U.S. Navy ships. Various 
measurements were taken during the experiments, including: low back muscle EMG 
(electromyography), foot volume changes, ground reaction forces, and discomfort felt by 
the worker. Hansen claims that the effects of working on a soft mat vice concrete are 
negligible. (This does not agree with the boatswain’s mate request for a rubber mat while 
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standing watch at the helm of a warship for several hours.) Hansen’s findings point to a 
greater effect of wearing soft shoes to reducing foot edema formation and heel impact by 
one half. They do admit that difference in their study between the soft shoe and the soft 
mat may be due to the “differences in the shock-absorbing characteristics and the 
thickness of the various materials (Hansen 1998).” 

C. DIRECT MEASUREMENTS OF VERTICAL GROUND REACTION 
FORCES 

The initial impact impulse force of heel strike during human gait can be measured 
directly using force plates and force sensors on the outside or inside of the shoe. By 
directly measuring the impulse force, characteristics of the impulse can be compared 
directly for quantitative analysis. An additional technique for quantitatively evaluating 
the effect of the initial impact impulse forces’ effect on the human body, which will not 
be reviewed here, was completed using accelerometers attached to several points on the 
body (Bresler an Frankel 1950, Voloshin and Wosk 1981, Morechi et al 1981, Wosh and 
Voloshin 1985, and Collins and Whittle 1989a). Vertical ground reaction force data 
collected from miniature insole load cells and data obtained from a force plate in a 
walkway agree closely, and both show the initial impact impulse force (King and Nakhla 
1986). 

1. Force Plates 

Bresler and Frankel (1950) provide one of the first thorough calculations of the 
forces and moments in the ankle, knee, and hip during level walking. They used a force 
plate that measured the location of the center of pressure, torque about die z-axis, and the 
vertical and horizontal forces applied to the foot. The output was recorded on an 
oscillograph tape. No indication of the sensitivity of the plate is given, and no impulse is 
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seen on any of the vertical component force graphs during the climb to the first of the 
typical double peaks. 

Today, the use of the force plate is well established, and two of the most 
commonly used are commercially available through Kistler Instrument Corporation and 
Advanced Mechanical Technology, Inc. (AMTI). Cavanagh et al. (1981) used a Kistler 
force platform with a sufficient sampling rate to compare the ground reaction forces of 
ten male subjects in barefoot, army boots, leather street shoes, and casual suede shoes. 
Eight subjects showed “well defined peaks between 0.38-0.85 of body weight (BW) 
occurring during the first 11 ms of contact.” The results show that the magnitude of the 
initial impact impulse force, in order of greatest to least, was barefoot, army boots, 
leather sole, and crepe sole (of the suede shoes). 

Rohrle et al. (1984) use optimization techniques with the data obtained from a 
force plate-video system to determine forces in the hip, knee and ankle joints. An 
extensive reference list, detailed description of the gait lab, description of the data 
processing techniques, and muscle attachment locations would prove to be useful as a 
reference to someone setting up a gait laboratory or processing data. The data displayed 
does not show the initial contact impulse force. 

Many force plate options are available for various sensitivities, force measuring 
ranges, natural frequencies, etc. Prices in 1998 varied from $5,000 to $18,000 per force 
plate. Complete systems that include the force plates and supporting hardware and 
software range from $15,000 to $24,000. AMTI uses strain gauges and Kistler uses 
piezoelectricity (Davis and DeLuca 1996). Davis and DeLuca provide an overview with 
many references of the use of gait analysis in clinical applications. 
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With more work and decreased cost, a good machine shop can manufacture a 
force plate using three or four piezioresistive triaxial strain gauges at an approximate cost 
of $1500 each. Additional equipment required that also should be available in a well 
equipped laboratory is amplifiers and A-D (analog-digital) converters. The final element 
would be the software required to process and record the H^ita 

In a study about the reliability of comparing ground reaction force data. Bates et 
al. (1984) caution that in order to statistically analyze gait data taken on different days, 
ten or more trials must be taken. Macellari (1996) uses data collected from a compound 
instrument consisting of a force plate and pressure platform to compare ground reaction 
forces with a pressure distribution. 

Chao et al. (1983) use parametric and non-parametric (Fourier series) 
representations to statistically analyze a normative database of 148 adults during level 
walking. They use a Kistler force plate at a sampling frequency of 100 Hz and showed 
only a small bump where the initial contact impulsive force is seen on other studies. 
Further, after the Fourier smoothing takes place, no analysis of the initial contact can be 
made. “It was found that sex-related variation is more significant than age-related 
variation in adult gait (Chao et al. 1983).” Kerrigan et al. (1998) also studied the 
difference between gender in joint biomechanics during walking. Although they 
concluded that there were "more similarities than differences" between males and females 
with respect to gait kinetics and kinematics, before initial contact "females had greater 
peak hip flexion and less knee extension (Kerrigan et al. 1998)." Initial impact impulse 
force does depend on the angle of limb approach (Simon et al. 1981). This thesis will 
assume no difference between sexes or ages. 
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2 . 


In-Shoe Force Sensors 


In-shoe force sensors with inexpensive and lightweight recorders have been used 
for collecting two to three hours of ground reaction force data. This data can then be 
downloaded into a computer and analyzed. Such in-shoe devices can be used to measure 
the initial impact impulse force as a subject walks on various materials in order to 
quantify the change in the impact force. In-shoe measuring devices are particularly 
appealing due to their low cost, portability, and the fact that a sensor inside the shoe is 
closer to the heel, and may be more representative of what is happening there, because 
the heel of the shoe effects the impulse force. This procedure would be especially useful 
to analyze several sailors aboard a modem warship to determine where the greatest forces 
occur and, therefore, where the most critical parts of the ship for compliant surface 
coating are found. 

Abu-Faraj et al. (1996) used Force Sensing Resistors (FSR) and an on-subject 
data recording device weighing 350 g to record two hours at 40 Hz of in-shoe pressures 
for the purpose of evaluating metatarsal and scaphoid pads. Dingwell et al. (1997) use an 
insole Personal Force Monitoring Device (PFMD) for recording ten hours of continuous 
ground reaction forces at a sample rate of 25 Hz. All on-person parts weigh 1.5 kg and 
“consist of a pair of capacitative force monitoring shoe insoles and signal amplifier 
(Electronic Quantification, Inc.), a Tattletale microprocessor (Onset Computer, Inc.) with 
a four megabyte PCMIA card for data storage, and an elastic waist belt to which these 
electronics are attached" (Dingwell 1997). The authors outline a method for quantifying 
the load bearing activities of the subjects. 
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D. ANALYTICAL MODELS OF GAIT 


Another option for the quantitative evaluation of initial impact impulse forces, 
and the direction chosen in this thesis, uses an analytical model. Several analytical 
models exist from finite element analysis to inverse kinematics and direct dynamics for 
the analysis of a human gait. Chapter V of this thesis addresses the specific calculations 
of one type of analytical model, namely the recursive Newton-Euler direct dynamics 
model. 

Many computer simulation models exist for the analysis of the human gait 
(Abdelnour et al. 1975, Siegler et al. 1982, Pandy 1987, Pandy and Berme 1988, 
Amirouche et al. 1990, and Zanchi and Cecic 1996, among others), but few, if any, look 
at the first few milliseconds of heel impact. Abdelnour et al. (1975) use experimental 
gait kinematics data and Lagrange’s form of d’Alembert’s principle (Kane’s equations) to 
compute joint forces and moments and ground reaction forces. A small peak does appear 
on the z-axis normalized foot force (vertical ground reaction force) figure; however it is 
not discussed in the paper. 

Amirouche et al. (1990) present "an all purpose algorithm used in the analysis and 
simulation of human locomotion." An initial configuration is set up, and different 
constraint equations are used to analyze various human movements. Ground reaction 
forces are computed without force plates. Kane’s equations are also used. The motion of 
the human is only tested for the swing phase of gait, and no analysis is made of the initial 
impact. 

Koozekanani et al. (1983) extended a recursive Newton-Euler inverse dyn ami fi 
model to direct dynamics. They demonstrate the technique on a simple open serial chain 
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model for planar postural dynamics. The shank, thigh, trank, and head angles of the 
simulation agree very closely to the experimental results. Pandy (1987) and Pandy and 
Berme (1988) apply a recursive Newton-Euler inverse dynamic model to the two phases 
of human gait (single and double support) separately, and no mention is given to the 
initial impact impulse force that connects the two phases. 

The models of human gait are typically subdivided into double limb support or 
stance and single limb support (Pandy and Berme 1988). During double limb support, 
both feet are on the ground and equations are used to simulate a closed kinematic chain, 
while an open kinematic chain is simulated during single support. 

T. McMahon et al. (1979), built an analytical expression to “predict separately the 
effect of track compliance on step length and ground contact time.” These effects were 
functions of track stiffiiess. The results were compared to experimental results. The 
results also showed that initial contact impulse force reaches nearly five times body 
weight during running on a hard surface, but that more compliant tracks can show a 
“marked attenuation (McMahon 1979).” Although this study was performed on runners, 
the impulse does exist during walking at various speeds. 

E. OTHER REVIEWS 

Wilson et al. (1997) used principal component analysis (PCA) on existing 
recorded data to determine statistical correlations between walking, running, rigid and 
mat footfall surfaces, gender, and arch indexes. PCA showed no effects of the different 
footfall surfaces. However, Wilson cautions that sometimes PCA may mask changes in 
response parameters, and a 24% reduction of the vertical load rate was seen for barefoot 
subjects walking on the matted surface as compared to the unmatted surface. 
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F. SUMMARY 

A qualitative evaluation of the effects of surface compliance on the initial impact 
impulse force of a human gait requires an accurate measurement or determination of 
characteristics of the impulse force. These characteristics can be made through direct 
measurements using force plates or miniature load measuring devices either in or on the 
outside of the shoe. Analytical models may also be used to estimate the impulse force 
and use this data for a qualitative comparison. An analytical model was chosen for this 
thesis due to cost and time constraints. Chapter IV of this thesis reviews mathematical 
models that most directly apply to the model used in this thesis. Although an analytical 
model cannot simulate the gait exactly, especially the initial impact, an analytical 
approximation would be useful for checking relative changes in the initial impact impulse 
force due to deck compliance. 
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IV. MATHEMATICAL MODELS 
A. INTRODUCTION 

In order to begin to model human gait to quantify initial impact forces, an 
understanding of dynamics and methods used for these calculations are essential. 
Dynamics is the study of the motion of bodies. It is divided into two areas of study, 
kinematics and kinetics. Kinematics looks at the motion of the body (position, velocity, 
and acceleration) without considering the forces on the body that caused the motion 
(Craig 1989). Kinetics relates the forces and moments acting on a body to its resulting 
motion. 

A kinematic model calculates a rigid body’s position (Cartesian space) and 
orientation (roll, elevation, and azimuth) given an adequate description of the body in 
terms of body segment lengths and joint angles. This type of calculation is called 
forward kinematics. Inverse kinematics does the reverse, finding joint parameters from 
the given orientation and position of body segments. Kinematics also includes the 
relationship between position, velocity, and acceleration within and through the given 
coordinate system(s). 

A “forward” or “direct dynamics” problem calculates the body’s motion from the 
given forces and moments acting on the body. “Inverse dynamics” calculates the forces 
or moments required to cause a given motion of the body. Dynamics in general is the 
union of kinetics and kinematics to understand and describe the relationship between 
position, velocity, acceleration, and orientation (in any coordinate system) based on the 
body’s parameters and the forces and moments acting on that body. 
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A human dynamic model may be as simple as an inverted pendulum with one 
degree of freedom or a more realistic model with 200 degrees of freedom (McGhee et al. 
1979). The complexity of the kinetic and kinematics calculations increases tremendously 
as the designer attempts to three dimensionally model individual muscles and the 
compliance and damping associated with joints, vis-a-vis a two-dimensional model of 
planar forces and moments with hard constraints in each of the joints. 

B. KINEMATIC MODELS 

Kinematic models of human gait enable the comparison of a healthy gait to a 
pathological gait. Inverse kinematic models are used by recording the position and 
orientation of subject through film or video recording to calculate angles, timing, and 
phase relationships. “Through advanced reasoning, which correlates the patients 
performance with normal phasic function, the primary defects can be differentiated from 
substitutive actions” (Perry 1992). Someone with enough experience or insight can be 
aided in a diagnosis of a pathological gait from analysis of kinematic data. Also, gait 
improvements during rehabilitation can be quantitatively made from kinematic data. 

Ju and Mansour (1988) used foot kinematic data to construct a planar foot model 
for use in a simulation involving the double limb support phase. Kinematics is also 
fundamental to dynamic models. 

C. DYNAMIC MODELS 

1. Direct Dynamic Models 

Direct dynamics solve for motion (orientation, position, velocity, and 
acceleration) of a rigid body based on the force and moment inputs acting on that body. 
Accurate forces and moments must be obtained in order to be used for calculations in a 
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direct dynamics human simulation. These forces and moments can be obtained from an 
inverse dynamics model, feedback control, or optimization techniques. Most models use 
the forces and moments obtained from inverse dynamics models as described in the next 
section (Abdelnour et al 1975 and Ju and Mansour 1988 among others). 

2. Inverse Dynamic Models 

Inverse dynamic models use the position, velocity, acceleration, and ground 
reaction force data obtained from video recording equipment, force plates, and 
appropriate computers and software, usually within a gait analysis laboratory, to 
determine the forces and moments at the ankle, knee, and hip (Bresler and Frankel 1950). 
These forces and moments can then be applied to a direct dynamics computer simulation 
or mathematical model of an articulated human body to determine the position, velocity, 
and accelerations. This movement is then compared to the original human movement to 
determine the accuracy of the model. This type of analysis has gone beyond purely 
academic exercises of understanding human gait to clinical analysis of pathological gait 
and posture. The nature of a problem evidenced by weak or over-used muscles is better 
understood by studying the pathological gait, and comparing it to a healthy gait. 

D. ARTICULATED RIGID BODY DYNAMICS 

A “link” is a rigid body connected to other link(s) in a chain by joints to form a 
"manipulator" or serial linkage mechanism (Craig 1989). Craig (1989) describes a link 
with four parameters: link length (a), link twist (a), link offset (d), and joint angle (0). A 
human body can be simulated as an n-link open chain articulated planar mechanism. The 
chain is “open” because the end-effector (head or torso) has no forces or moments acting 
at its free end. A closed chain has both ends constrained such as when modeling both legs 
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during the double stance phase of gait. The mechanism is planar because the joints are 
modeled as revolute joints and twist angles are zero degrees. 

Using kinematics with the Denavit-Hartenberg method of link transformation, the 
coordinate system of link i of an n-link open chain articulated rigid body with respect to 
i-l’s coordinate system is found with a A matrix calculation. The A-matrix is defined as 
(Davidson 1993) 


cos 6i 



sin^,. 

0 

0 


- sin cos or, sin 0,. sin a, a-cos0- 

cos^jCosa, -cos^,sin«,. a,sin^ 

sin QT; cos a,. d,. 

0 0 1 


(Eq. 4.1) 


where 0, a, d, and a are the link parameters defined above. 

The general equations of motion for a system of rigid bodies connected together 
with rotary joints and with control torque, T, acting at each joint is (Koozekanani 1983) 
given by Equation 4.2. In what follows, bold type indicates vectors. Thus, 

j{6^=^A{p,$)+BT (Eq. 4.2) 

where 


J( 6) is the system inertia matrix, 

A is a vector of centrifugal, Coriolis and gravitational forces, 

B is the control distribution matrix, 

^,^,andtfare vectors of joint angles, joint rates, and joint accelerations, 
respectively, and T is the vector of joint torques. 

Solving Equation 4.2 for T, 

T = B-^J{0)e - B-^A{0,0 ). (Eq. 4.3) 
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The inverse dynamics problem solves for the joint torque, T, given joint angles, rates and 
accelerations (Equation 4.3). 

Conversely, the joint acceleration can be directly solved for with a given torque. 
This acceleration can be integrated once to solve for velocity and twice for position. 
Although this solution seems straightforward, the complexity for a multiple links model 
makes using it impractical (Koozekanani et al.l983). A recursive form of this equation 
was developed by Stepanenko and Vukobratovic (1976) for an inverse dynamic model. 
Koozekanani et al. (1983) applied this method to a direct dynamics model and used a 
single open serial chain model for postural dynamics as an example. 

1. Recursive Inverse Dynamics Model 

As stated earlier, inverse dynamics involves solving for forces and moments given 
the position, velocity and acceleration of the body. For an n-link open chain articulated 
planar mechanism as demonstrated by Koozekanani et al. (1983), this method can be 
solved recursively. 

Accelerations of each link are calculated from the inboard to the outboard link 


using Equations 4.4 and 4.5. The results are: 


Xi = ’x^_^ - - d._^ )(^‘,-_i sin ^/-i + Oh cos sin 6^ + 9^ cos 0,.) (Eq. 4.4) 


z. = -z._, - - d._^ cossin e,_,)- d^ie^ cos 9^ - 9f sin 9^) (Eq. 4.5) 


Forces and moments are then calculated from the outboard link to inboard using 


equations 4.6,4.7, and 4.8. For an open chain, the end link is open and, therfore, has no 
forces or moments acting on its outboard end. Figure 4.1 shows the free-body diagram 


for one of these links. 
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(Eq. 4.6) 


-> 




m.x. 


F, =F_ +m,x, 

X,- JC/+1 I / 


+ m,.z,.-m,.g 

® X^ => 2 ; = 7 ;+, - (/, - 1 /, )sin 0,. + F^^^f /,. - cf; )cos 0,. + 

J fii - sin^, + F^di cosSi 


(Eq.4.7) 


(Eq. 4.8) 



Figure 4.1. Free-body diagram for link i of an n-link serial open-chain articulaed 

planar mechanism 


2. Recursive Direct Dynamics Model 

Koozekanani et aJ. (1983) demonstrated the extension of the recursive inverse 
dynamics method to a recursive direct dynamics model for an n-link serial open-chain 
articulated planar mechanism on a postural control model. Solving Equation 4.2 for 
angular acceleration, the Lagrangian formulation of linkage dynamics of the form 

0 = 7"‘A + J~'BT (Eq. 4.9) 

will eventually be reduced to a simpler equation of the form 

e = C-\T-T„). (Eq.4.10) 
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First let Equation 4.3 be of the form, 

T = C0-D (Eq.4.11) 

where C = B~^J = [CjCj • • •C'„] and D = . If each link has an acceleration of zero, 

$ = (0,0,-",0)’", then solving equations 4.4 and 4.5 from link 1 to link n, and Equations 
4.6 through 4.8, from link n to link 1, the torque vector,ro is determined. From Equation 
4.11, To = -D , and Tq is called the "equilibrium" torque vector of length n. 

Next, step through each link with unit acceleration, or ^ = (l,0,-",0)^. The 
torque vector solved here will be called unit acceleration torque, Ti. Since, from 
Equation4.11, T, =Ci-D = C,+ro, then 

C,=T,-T,, (Eq.4.12) 

where Ci is the first column of the C matrix in Equation 4.11, or generally, 

Ci=Ti-To (Eq.4.13) 

for ^ = (0,0,-",l," 0,0)^. Inverting this C matrix and solving for angular acceleration. 
Equation 4.10 results where T is any arbitrary torque. As stated above, the angular 
velocity and position can then be determined through numerical integration. 

The arbitrary torque vector, T, can be obtained from the data calculated using 
inverse dynamics equations and data from observations during a gait analysis laboratory 
experiment. Koozekanani et al. (1983) used linear feedback in the form of Equation 4.14 
for the input torque vector to achieve postural control. 

T = -k^(0-e^)-k^(0) (Eq. 4.14) 


where and k ■ are the joint feedback gains, and So is the positional goal. 

a 6 
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E. SUMMARY 


In order to determine the forces and moments at each joint, kinematic data must 
be available and inverse dynamics equations used for the calculations. In the case of 
postural control, a recursive direct dynamics model can be solved by using an applied 
torque vector calculated from linear feedback. These techniques can be applied to a four- 
link open-chain serial articulated planar mechanism for the purpose of studying the initial 
impact phase of human gait. 
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V. FOUR-LINK HUMAN MODEL 


A. INTRODUCTION 

The analytic computer model used in this thesis contains a foot and an open chain 
planar serial linkage as shown in Figure 5.1. The foot is treated as a three-dimensional 
rigid body. The shank, thigh, and upper body are each three-dimensional links with 
revolute joints. (Only two-dimensional dynamics were used for the links.) The linkage is 
connected to the foot using a fourth massless link, "linkO," that “transfers” forces, 
moments, position, velocities, and accelerations between the foot and the shank. LinkO 
has a twist angle of 90° in order to “twist” the inboard joint z-axis of revolution to an 
outboard link axis of rotation (ankle rotation) about the y-axis. 



Figure 5.1. Four-mass sagittal plane model for the body from Koozekanani et al. 

(1980). 
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The foot is treated as a rigid body. The foot’s position and Euler angles are used 
to properly position linkO while the foot’s linear accelerations are used for the calculation 
of the linear acceleration of linkl. The recursive Newton-Euler inverse-dynamics 
technique as described earlier is used to update the linear velocities of the remaining 
links. The forces and moments that act on the foot are then calculated, and the foot is 
updated. This cycle repeats until the loop ends. 

This four-link inverted pendulum attached to a rigid body is an accurate 
representation of a human in a two-legged jump. The initial position of each link and 
body can be specified. The goal position can also be specified. With the correct initial 
conditions given, this model can satisfactorily represent the first few tenths of a second of 
heel strike during human gait. The importance of this phase of the gait with respect to 
knee injuries associated with articular cartilage changes that lead to chondromalacia and 
osteoarthritis has been shown earlier. 

The coordinate system used is one with x-axis to the right (north), z-axis is down 
(down), and the y-axis is out of the paper (east) as shown in Figure 5.2. 



Figure 5.2. Coordinate system for four-link human 
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The anthropometric (dimension, weight, and moment of inertia) values of the foot 
and each link were obtained from Verstraete (1988), Knudson (1980), and Pandy (1987). 
Table 5.1 lists the values used in the model. 



Mass 

(slugs) 

Length 

(ft) 

Mass Moment of Inertia 
(slugs-ft^) 

Upper Body (link3) 

3.75400 

2.240 

1.0000 

Thigh (link2) 

0.58740 

1.447 

0.0776 

Shank (linkl) 

0.25176 

1.550 

0.0372 

Foot 

0.08112 

0.492 

0.0280 


Table 5.1. Anthropometic data from Knudson (1980 pg. 21,34). 


B. FOOT DYNAMICS 

The foot’s direct dynamics calculations are accomplished through the computer 
program titled "euler-angle-rigid-body" in Appendix A. This code is a three-dimensional 
rigid body direct dynamics solver for ANSI Common LISP. The initial values of all state 
variables were set in the initial form command line in each corresponding slot of the 
defining class function within the LISP program. The state vector (position, orientation, 
and body coordinate referenced linear and angular velocities) is sent to a Heun 
integration routine. 

The forces and moments that are acting on the foot are calculated in the program 
file named “foot dynamics” (Appendix A). Figure 5.3 shows the free body diagram 
representing the foot. The quantities Exi, Fzi, and Mi are the forces and moments acting 
on the foot by the links. The forces acting at the heel and toe are reaction forces due to 
the spring and damping constants based on the mechanical properties of the ground. The 
recursive dynamics routine for the four inverted links use the earth-coordinate x-direction 
and z-direction accelerations calculated from the dynamics equation. 
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C. LINK DYNAMICS 

The links’ motions are calculated using the recursive Newton-Euler routine 
described in Chapter IV. After the accelerations of the foot are calculated for the given 
time step, these accelerations are used as inputs for link 1. Figure 5.4 shows a link free 
body diagram. The links’ parameters for proper kinematic calculations are listed in 
Table 5.2. 



Figure 5.4. Link free body diagram. 


Link 

link length (a) 

link twist (a) 

link offset (d) 

joint angle (0) 

Upper Body (link 3) 

2.240 

0 ° 

0 

03 

Thigh (link 2) 

1.447 

0 ° 

0 

02 

Shank (link 1) 

1.550 

0 ° 

0 

01 

Foot (link 0) 

0.1905 

-90° 

-0.2286 

0 ° 


Table 5.2. Link Parameters. 
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The distance to the center of gravity (dO was assumed to be one-half of the link 
length. Knudson (1980) provides a much more accurate placement of the center of 
gravity for human limbs. 

The foot is inboard to link 1, therefore, the foot’s linear acceleration values are 
sent to link 1 for the calculations to be made of the rest of the links. Forces and moments 
that are calculated at the base of link 1 are forces and moments that act on the foot. The 
foot’s dynamics encorperate these for the next time step’s calculation of linear 
acceleration. 

D. THE MISSING LINK 

The most critical (and the most challenging) aspect of modeling the human gait 
using the methods cited above was connecting the foot to the shank link. This thesis 
arbitrarily chose to use the forces (Fxi and Fzi) calculated during the equilibrium torque 
vector calculation to be the forces that act on the foot. The torque acting on the foot is 
the applied torque from the postural control feedback. The linear acceleration inputs for 
the inboard link of the shank are the linear accelerations of the foot. Both the foot and 
the inverted link system dynamics calculations were solved independently at each time 
step. The resulting dynamic model is not completely mathematically correct, but a crude 
approximation, as this method was chosen arbitrarily to test the model. 

Two mathematically correct choices could have been made. The two methods, 
soft and hard constraints, would allow simultaneous solving of the link system and the 
foot. First, soft constraints could be used to connect the links to the foot. Alternatively, 
in order to fully utilize the capabilities of the Denavit-Hartenberg method, the foot can be 
simulated using hard constraints as a pair of sliding-link joints together with one rotary 
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joint. The foot will then have 6 degrees of freedom, and the entire system can be solved 
simultaneously. Due to lack of time, this was not done in this thesis. 

E. GROUND 

The ground is modeled as a rigid body forming the x-y-plane at the origin of the 
z-axis. As any part of the foot begins to penetrate the “ground,” a reaction force 
corresponding to its stiffness and damping acts on that part of the foot (Figure 5.5). 
Table 5.3 lists the stiffness of various surfaces (McMahon et al. 1979). The damping 
coefficients used in this thesis correspond to critical damping based on the stiffness. 


Surface of "Soft” Deck 


ke < =\ ki ke , =\ ki 




Rigid Ground 


Figure 5.5. Surface stiffness and damping 


Material 

Stiffness (Ibf/ft) 

Concrete, asphalt 

300,000+ 

Packed cinders 

200,000 

Board tracks 

60,000 

Experimental wooden track 

133,333 

Experimental wooden track 

6857 

Pillow tack at 1.67g 

985 


Table 5.3. Stiffness of various surfaces 


F. ANSI COMMON LISP 

The ANSI Common LISP (Graham 1996) code was used for all progr ammin g of 
the four-link-human model. LISP (LISt Processor) has the advantage of being an object 
oriented programming (OOP) language with the ability to test individual functions as 
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they are defined. This feature permits a "bottom up" programming style which is very 
effective in complex system modeling. Use of OOP enables the programmer to write 
methods and functions that operate on and with the attributes of objects. Subclasses are 
able to inherit these attributes from their parent. Proponents argue that OOP is more 
analogous to the way a human thinks and therefore more intuitive than non-OOP. The 
results of this thesis demonstrate the versatility of OOP. The same program can 
demonstrate foot dynamics and postural control independently or together. The 
programmer only has to change a top-level function to use either or both. 

Fundamental to understanding and programming in OOP is a class and object 
structure. Figure 5.6 shows the class and object structure of the four-link human. The 
four-link human was modified from Davidson’s model of the aqua-robot (Davidson 
1993). 

G. SUMMARY 

This chapter presents the analytic computer model that is used to solve for the 
initial impact impulse forces during human gait. Anthropometric data for the model and 
ground characteristics are detailed. A brief description of object-oriented programming 
and the structure of this model is also presented. 
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VI. RESULTS 


A. INTRODUCTION 

The four-link human model was tested incrementally. One advantage of using 
LISP is that it allows the programmer to test each component separately and/or together. 
Only a different top-level function needs to be written for each case. This chapter is 
broken down into three sections as testing of the model was completed in stages. First, 
the foot was tested without the rest of the body linkage by setting and dropping it 1 cm 
with various initial conditions onto a surface. Next, the recursive direct dynamics 
calculations were tested using postural control. Finally, the two steps were brought 
together in the third section for testing the four-link human in the role of initial impact 
impulse force during human gait. 

B. FOOT RIGID BODY DYNAMICS 

1. Foot on ’’Soft" Surface 

Figure 6.1 is the foot by itself sitting on the ground. Figure 6.2 is a vertical 
ground reaction force versus time plot as the foot settles into the ground. The foot was 
placed just touching the soft surface (Zinit = -0.114286 ft.). The final position is slightly 
lower based on the weight of the foot and the spring constant of the ground 
(Zfinai = -0.11218 ft.). The center of gravity of the foot is 0.114286 ft above the bottom of 
the foot. The z-positions are negative because the ground is the coordinate axis with z- 
down being positive. For this "soft" surface, = 1000 Ibf/ft, and = 20 Ibf/ft/sec The 
foot dynamics routine was run for 0.2 seconds. 
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Foot - Looking West 



4, Vertical GRF vs. Time 



Figure 6.1. Foot on Soft surface. Figure 6.2. Fz versus time. 

2. Foot Drop onto Surface 

The next two figures (6.3 and 6.4) show the foot being dropped from the height of 
0.0328 ft (1 cm) and the resulting vertical ground reaction force, respectively. For this 
"soft surface," k, = 1000 Ibf/ft, and k. = 20 Ibf/ft/sec. 


Figure 6.3. Foot drop from 1cm. 


Figure 6.4. Vertical GRF vs. time. 





















3. Foot with Initial Angle Drop onto Surface 

Figure 6.5 is the final step in testing the foot dynamics with an initial height and 
angle that models the foot just before it begins its free-fall. The heel is 1 cm above the 
ground. The angle of the foot is ^ = -0.32175. The center of gravity is moved to 
properly position the heel (Zcg = - 0.2376). Figure 6.6 graphs the vertical ground reaction 
force versus time. 


^ Vertical GRF vs. Time _ HlilO 



Figure 6.5. Foot fall with angle. Figure 6.6. Vertical GRF vs. time. 


















C. RECURSIVE NEWTON-EULER DIRECT DYNAMICS 
1. Postural Control with Foot Constrained 

Figures 6.7 and 6.8 demonstrate the postural control ability of this model. The 
foot remains in its initial position and does not move. The model simulates 
Koozekanani's et al (1983) linear feedback postural control model. Stable linear 
feedback control gains are kg = (500,100,35)’’ and = (500,100,30)^. Both Figures 

show the goal position, the starting position, and the program stop position of the human 
model. 



38 
































2. Postural Control with Foot on “Soft” Surface 

Figures 6.9 through 6.12 show the four-link human under postural control on a 
compliant surface for two different postures. Figures 6.9 and 6.10 are for a "bent" 
configuration, and Figures 6.11 and 6.12 are for a stiff or more upright stance. The gains 
were kept the same and the spring constant for the ground was = 1000. 


^ Plotter-window 


f 


0.1 


time (sec) 


Figure 6.9. GRF vs. Time 



Figure 6.10. Postural control on soft 
surface for a bent stance 



surface for upright stance. 
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D. FOUR-LINK HUMAN DYNAMICS 

1. Drop Human 

Figure 6.13 shows the vertical ground reaction force of the four-link human 
"dropped" from just above the ground. No postural control was initiated during or after 
the drop. 

2. Drop Test with Initial Conditions and Goal 

Figure 6.14 shows the four-link human in the vertical position with a four-link 
human in the proper position for heel strike (Perry 1992). 



Figure 6.13 Vertical GRF vs. Time of Figure 6.14. Proper heel strike position, 
dropped four-link human without 
postural control. 
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3. Heel Strike in Human Gait 

Figure 6.15 shows an unstable first few milliseconds of a dropped four-link 
human with angled foot and postural control. 



E. SUMMARY 

These results have shown the capabilities of the four-link human model and the 
ability of the programming language to test parts of or the whole model. The motion of 
the foot and links and the vertical ground reaction force plots seem to reflect a logical 
reality. However, before the model can be used to quantitatively evaluate deck surfaces, 
a more accurate hard or soft constraint joint must be used between the foot and the links. 
The model must then be independently verified through experimentation or another 
model. 
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VII. SUMMARY AND CONCLUSIONS 

A. SUMMARY 

The initial impact impulse force associated with human gait has been postulated 
to be a leading cause of disability of sailors. Placing a softer material between the foot 
and the hard steel deck should attenuate this impulsive force thereby reducing injuries, 
loss of training, and disabilities and the costs associated with them. The amount of 
attenuation or other changes to the characteristics of the initial impact impulse force must 
be quantified before decisions may be made as to the direction the U.S. Navy with 
respect to applying "softer" coating to deck as a preventative measure. 

B. CONCLUSIONS 

A tool has been developed to begin to quantify the ability to evaluate a softer 
material to reduce the impulse force of the initial contact or heel strike during human gait. 
However, as previously discussed, the connection between the foot and body used in this 
thesis is only an approximation used for code development, and is not mathematically 
correct. Consequently, simulation needs to be improved in this regard and then verified 
by independent simulation or experimental observation. 

C. RECOMMENDATIONS 

Throughout the development of this model, several assumptions were made and 
questions raised. The following recommendations are presented to improve the model, 
clarify the problem, and achieve an acceptable answer. 

First, in order to improve the model, a mathematically correct mode for linking 
the foot to the body must be developed. Also, the model should be modified to include 
commanded velocities and initial condition velocities. The rest of the gait can then be 
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modeled. The second leg, arms and head could also be added. The foot, being the 
interaction point between the body and the ground, can be simulated with better 
geometry, and possible two links itself with a flexor spring and damping. Add a spring 
for the tissue of the heel (D Andrea et al. 1997) and a shoe (especially the spring and 
damping coefficients). Imagine a rigid foot hitting a steel deck. This model currently 
seems like a steel plate hitting the surface. The testing that can be accomplished with this 
program can be made more user friendly by building a graphical user interface (GUI) that 
would adjust initial positions, gains, spring constants, human properties, initial position, 
end position, etc. The ALLEGRO CL® "Dynamic Object-Oriented Progr ammin g 
System" (Franz Inc.) contains a useful tutorial to design, set up, and use a GUI for ANSI 
Common LISP programming. 

Secondly, in order to clarify the problem, updated knee injuries statistics relating 
to articular cartilage damage within the Navy should be obtained in order to validate the 
cost associated with furthering this research. The Helmkamp and Coben (1985) study 
cited in Chapter n looked at sailors only after five to seven years of service. These types 
of ^^juries worsen with time. A study that looks at 15 to 20 years of service would also 
be helpful in determining the extent of these types of knee and joint problems. 
Additionally, because back pain has been tied to the initial impact impulse force (Collins 
and Whittle 1989a), the study should include back pain. 

It would be useful to conduct a study equipping sailors with the low cost but 
accurate in-shoe force measuring device. Such measuring devices could be worn for 
extended periods of time in port and underway to determine the loading on the human 
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body during shipboard life. In such a study, special emphasis should be placed on 
walking through hatches, transitioning off ladders, and climbing ladders. 

Finally, in order to validate the model, a partnership can be made with an existing 
gait analysis laboratory. It would be especially valuable to put a hatch on a walkway 
equipped with a force plate and measure the ground reaction forces felt when a person 
steps through a smaller opening in which he or she must duck. Such motion requires a 
greater free fall and a more violent collision with the ground. Additionally, a ladder 
should be constructed on the runway. The ladder should be positioned so that the foot 
coining off of the ladder lands on the force plate (possibly two force plates, one for each 
foot). 
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APPENDIX A: ANSI COMMON LISP CODE 


; File :robot-kinematics.cl 

; Author :Dr. R. McGhee 

; :Naval Postgraduate School 

; :Monterey, CA 93943 

; Summary :Matrix and Vector Operations 

, ieie-k*’kif**ickieic-k*'k-k**ie * * ******************************************************* 

{defun transpose (matrix) ;A matrix is a list of row vectors. 

(cond ((null (cdr matrix)) (mapcar 'list (car matrix))) 

(t (mapcar 'cons (car matrix) (transpose (cdr matrix)))))) 

(defun dot-product (vector-1 vector-2) ;A vector is a list of niunerical atoms, 
(apply '+ (mapcar '* vector-1 vector-2))) 

(defun cross-product (vector-1 vector-2) 

(let ((xl (first vector-1)) (yl (second vector-1)) (zl (third vector-1)) 

(x2 (first vector-2)) (y2 (second vector-2)) (22 (third vector-2))) 

(list (- (* yl z2) (* y2 zl)) (- {* x2 zl) (* xl z2)) 

(- (* xl y2) (* x2 yl))))) 

(defun vector-magnitude (vector) (sqrt (dot-product vector vector))) 

(defun post-multiply (matrix vector) 

(cond ((null (rest matrix)) (list (dot-product (first matrix) vector))) 

(t (cons (dot-product (first matrix) vector) 

(post-multiply (rest matrix) vector))))) 

(defun pre-multiply (vector matrix) 

(post-multiply (transpose matrix) vector)) 

{defun matrix-multiply (matrixl matrix2) 

(cond ((null (rest matrixl)) (list (pre-multiply (first matrixl) matrix2))) 
(t (cons (pre-multiply (first matrixl) matrix2) 

(matrix-multiply (rest matrixl) matrix2))))) 

(defun chain-multiply (L) ;L is a list of names of conformable matrices, 

(cond ((null (cddr L) ) (matrix-multiply (eval (car L) ) (eval (cadr L) ))) 

(t (matrix-multiply (eval (car L) ) (chain-multiply (cdr L) ))))) 

(defun cycle-left (matrix) (mapcar 'row-cycle-left matrix)) 

(defun row-cycle-left (row) (append (cdr row) (list (car row)))) 

(defun cycle-up (matrix) (append (cdr matrix) (list (car matrix)))) 

(defun unit-vector (one-coliimn length) ;Column coiint starts at 1. 

(do ((n length (1- n)) 

(vector nil (cons (cond ((= one-col\amn n) 1) (t 0)) vector))) 

((zerop n) vector))) 

(deftin unit-matrix (size) 

(do ((row-n\unber size (1- row-ntimber)) 

(I nil (cons (unit-vector row-number size) I))) 

((zerop row-number) I))) 

(defun concat-matrix (matrixl matrix2) 

(if matrixl (cons (append (first matrixl) (first matrix2)) 

(concat-matrix (rest matrixl) (rest matrix2))))) 

(defun augment (matrix) 


47 




(concat-matrix matrix (unit-matrix (length matrix)))) 


(defun normalize-row (row) (scalar-multiply (/ 1.0 (first row)) row)) 


(defun scalar-multiply (scalar vector) 

(cond ((null vector) nil) 

(t (cons (* scalar (first vector)) 

(scalar-multiply scalar (rest vector)))))) 


(defun 

(do* 


solve-first-column (matrix) ;Reduces first colximn to (10 ... 0). 

((remaining-row-list matrix (rest remaining-row-list)) 

(first-row (normalize-row (first matrix))) 

(answer (list first-row) 

(cons (vector-add (first remaining-row-list) 

(scalar-multiply (- (caar remaining-row-list)) 
first-row)) 


answer))) 

((null (rest remaining-row-list)) (reverse answer)))) 


(defun vector-add (vector-1 vector-2) (mapcar '+ vector-1 vector-2)) 

(defun vector-subtract (vector-1 vector-2) (mapcar '- vector-1 vector-2)) 

(defun matrix-subtract (matrix-1 matrix-2) 

(mapcar #'vector-subtract matrix-1 matrix-2)) 


(defun subtract-unit-matrix (square-matrix) 

(matrix-subtract square-matrix (unit-matrix (length square-matrix)))) 

(defun sum-of-elements-squared (matrix) 

(apply '+ (mapcar #'dot-product matrix matrix))) 

(defun rms-inverse-error-metric (matrix approximate-inverse-matrix) 

(let* ((M matrix) (M-inv approximate-inverse-matrix) (n (length M) ) 

(error-matrix (subtract-unit-matrix (matrix-multiply M M-inv) )) 
(S (sum-of-elements-squared error-matrix))) 

(/ (sqrt S) n))) 


(defun first-square (matrix) ;Returns leftmost square matrix from argument, 

(do ((size (length matrix)) 

(remainder matrix (rest remainder)) 

(answer nil (cons (firstn size (first remainder)) answer))) 

((null remainder) (reverse answer)))) 

(defun firstn (n list) 

(cond ((zerop n) nil) 

(t (cons (first list) (firstn (1- n) (rest list)))))) 

(defun max-car-firstn (n list) 

(append (max-car-first (firstn n list)) (nthcdr n list))) 

(defun matrix-inverse (matrix) 

(do* ( (M (max-car-first (augment matrix)) 

(max-car-firstn n (cycle-left (cycle-up M)))) 

(n (1- (length matrix)) (1- n)) 

(exit-flag (= 0 (caar M) ) ^{= 0 (caar M)) )) ;Prevents division by zero, 
((or (minusp n) exit-flag) (if (not exit-flag) (first-square M))) 

(setf M (solve-first-column M)))) 

(defun max-car-first (matrix) ;This function finds row with largest first 

(cond ((null (cdr matrix)) matrix) ;element and moves it to top of matrix, 
(t (if (> (abs (caar matrix)) 

(abs (caar (max-car-first (cdr matrix))))) matrix 
(append (max-car-first (cdr matrix)) (list (car matrix))))))) 
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(defiin dh~matrix (rotate twist length translate) 

(let ((cosrotate (cos rotate)) (sinrotate (sin rotate)) 

(costwist (cos twist)) (sintwist (sin twist))) 

(list (list cosrotate (- (* costwist sinrotate)) 

(* sintwist sinrotate) (* length cosrotate)) 

(list sinrotate (* costwist cosrotate) 

(- (* sintwist cosrotate)) (* length sinrotate)) 

(list 0. sintwist costwist translate) 

(list 0. 0. 0. 1.)))) 

(defun homogeneous-transform (orientation position) 

(let* ((roll (first orientation)) (elevation (second orientation)) 

(azimuth (third orientation)) (x (first position)) 

(y (second position)) (z (third position)) 

(spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 

(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi)) 

(+ (* cpsi sth cphi) (* spsi sphi)) x) 

(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 

(- (* spsi sth cphi) (* cpsi sphi)) y) 

(list (- sth) (* cth sphi) (* cth cphi) z) 

(list 0. 0. 0. 1.)))) 

(defun inverse-H (H) ;H is a 4x4 homogeneous transformation matrix, 

(let* ((minus-P (list (- (fourth (first H))) 

(- (fourth (second H))) 

(“ (fourth (third H))))) 

(inverse-R (transpose (first-square (reverse (rest (reverse H)))))) 
^nverse-P (post-multiply inVerse-R minus-P))) 

(append (concat-matrix inverse-R (transpose (list inverse-P))) 

(list (list 0001))))) 

(defiin rotation-matrix (euler-angles) 

(let* ((roll (first euler-angles)) (elevation (second euler-angles)) 
(azimuth (third euler-angles)) 

(spsi (sin azimuth)) (cpsi (cos azimuth)) (sth (sin elevation)) 

(cth (cos elevation)) (sphi (sin roll)) (cphi (cos roll))) 

(list (list (* cpsi cth) (- (* cpsi sth sphi) (* spsi cphi)) 

(+ (* cpsi sth cphi) (* spsi sphi))) 

(list (* spsi cth) (+ (* cpsi cphi) (* spsi sth sphi)) 

(- (* spsi sth cphi) (* cpsi sphi))) 

(list (- sth) (* cth sphi) (* cth cphi))))) 

(defun body-rate-to-euler-rate-matrix (euler-angles) 

(let* ((roll (first euler-angles)) (elevation (second euler-angles)) 

(sth (sin elevation)) (cth (cos elevation)) (tth (tan elevation)) 
(sphi (sin roll)) (cphi (cos roll))) 

(list (list 1 (* tth sphi) (* tth cphi)) 

(list 0 cphi (- sphi)) 

(list 0 (/ sphi cth) (/ cphi cth))))) 

(defun rad-to-deg (angle) (* 57.29577951308232 angle)) 

(defun deg-to-rad (angle) (* 0.017453292519943295 angle)) 
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(defun euler-step (x time time-step) ;x is list representation of state vector 
(vector-add x (euler-increment x time time-step))) 

(defun euler-increment (x time time-step) 

(scalar-multiply time-step (derivative x time))) 

(defun heun-step (x time time-step) 

(vector-add x (scalar-multiply (* time-step .5) 

(vector-add (derivative x time) (derivative (euler-step x time time-step) 

(+ time time-step)))))) 

(defun RK4-step (x time time-step) 

(let* ((dt (* time-step .5)) (delta-xO (euler-increment x time dt)) 

(delta-xl (euler-increment (vector-add x delta-xO) (+ time dt) dt)) 
(delta-x2 (euler-increment (vector-add x delta-xl) (+ time dt) dt)) 
{2dx2 (scalar-multiply 2 delta-x2)) 

(delta-x3 (euler-increment (vector-add x 2dx2) (+ time time-step) dt)) 

(sum (RK4-sum delta-xO delta-xl delta-x2 delta-x3)) 

(RK4-increment (scalar-multiply 1/3 sum))) 

(vector-add x RK4-increment))) 

(defun integration-step (method x time time-step) 

(cond ((equal method 'euler-step) (euler-step x time time-step)) 

((equal method 'heun-step) (heun-step x time time-step)) 

({equal method 'RK4-step) (RK4-step x time time-step)))) 

(defun RK4-sum (delta-1 delta-2 delta-3 delta-4) 

(let* ((dl delta-1) (d2 delta-2) (d3 delta-3) (d4 delta-4) 

(suml (vector-add dl d4)) (sum2 (vector-add d2 d3))) 

(vector-add suml (scalar-multiply 2 s\im2)))) 
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. *****************************’*'**********************************-************ 

; File :plotter.cl 

; Author :David A. Bretz 

; :Naval Postgraduate School 

; :Monterey, CA 93943 

; Summary :Draws line in plot window instance form old-value to new-value 

; :It is not user friendly, ie., it must be changes manually for 

; :different x (horizontal) and y (vertical) values. 

; :A11 functions in this file are taken from Dr. R. McGhee's 

; :Strobe-Camera 

. **************************************************************************** 


(defclass plotter () 

((plotter-window 

:accessor plotter-window 
:initform (open-stream 'bitmap-window 
*lisp-main-window* 

:output 

: window-exterior 

(make-box 720 300 1020 650) 

:title "Plotter-window")) 

(old-value 
:initform '(0 0) 

:initarg :old-value 
:accessor old-value) 

(new-value 
rinitform '(0 0) 

:initarg :new-value 
:accessor new-value))) 

(defmethod plot ((plotter plotter) x-data y-data) 

(setf (new-value plotter) (list x-data y-data)) 

(draw-line-in-window (plotter-window plotter) 

10 

(old-value plotter) (new-value plotter)) 
(setf (old-value plotter) (list x-data y-data))) 


(defun draw-coordinate-axes (window) 

(draw-line window (make-position 20 280) ; was 20 150 

(make-position 280 280)) ; was 280 150 
(draw-line window (make-position 20 20) 

(make-position 20 280))) 

(defun scale-point-coordinates (x-y-list enlargement-factor) 

(let ((x (first x-y-list)) (y (second x-y-list))) 

(list (+ 20 (round (* enlargement-factor x 20))) 

(+ 280 (round (* (- y) )))))) ;was + 150 

(defun draw-line-in-window (window enlargement-factor start end) 

(let ((scaled-start (scale-point-coordinates start enlargement-factor)) 
(scaled-end (scale-point-coordinates end enlargement-factor))) 
(draw-line window 

(make-position (first scaled-start) (second scaled-start)) 
(make-position (first scaled-end) (second scaled-end))))) 
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. **************************************************************************** 

; File :euler-angle-rigid-body.cl 

; Author :Dr. R. McGhee 

; :Naval Postgraduate School 

; :Monterey, CA 93943 

; Siinunary :Rigid body class written and implemented by Dr. R. McGhee. 

. **************************************************************************** 

(defclass rigid-body () 

((euler-angles ;The vector (psi theta phi) = (roll elevation azimuth), 

rinitform '{0 0 0) 

:initarg :euler-angles 
:accessor euler-angles) 

(cg-position ;The vector (xe ye ze) = (north east down). 

zinitform '(000) 

:initarg :cg-postion 
:accessor cg-position) 

(body-coord-angular-velocity ;The vector (p q r) = (roll-rate pitch-rate 
zinitform '(0 0 0) ; yaw-rate) 

zinitarg :body-coord-angular-velocity 
z accessor body-coord-angular-velocity) 

(body-coord-linear-velocity ;The vector (u v w) in body coordinates, 
zinitform '(0 0 0) 

zinitarg zbody-coord-linear-velocity 
:accessor body-coord-linear-velocity) 

(moment-of-inertia-matrix ;Moments of inertia about center of gravity, 
zinitform (unit-matrix 3) 
zinitarg zmoment-of-inertia-matrix 
z accessor moment-of-inertia-matrix) 

(mass 

z initf orm 1 
zinitarg zmass 
zaccessor mass) 

(node-list ;(x y z 1) in body coord for each node. Starts with (0001). 
zinitform '((0 0 0 1) (0 0 0 1) (0 0 0 1)) 
zinitarg z node-list 
zaccessor node-list) 

(polygon-list 
zinitform '(1 2) 
zinitarg zpolygon-list 
zaccessor polygon-list) 

(transformed-node-list ; (x y z 1) in earth coord for each node in node-list. 

zaccessor transformed-node-list) 

(H-matrix 

zinitform (unit-matrix 4) 
zaccessor H-matrix) 

(clock-tick-count 
zaccessor clock-tick-count) 

(time-stamp 
zinitform 0 

zaccessor time-stamp))) 

(defmethod initialize ((body rigid-body)) 

(setf (transformed-node-list body) (node-list body) 

(clock-tick-count body) (get-internal-real-time) 

*inverse-mass* (/ 1 (mass body)) 

*1* (moment-of-inertia-matrix body) 

*I-inv* (matrix-inverse *1*))) 
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(defmethod move {(body rigid-body) orientation position) 

(setf (H-matrix body) (homogeneous-transform orientation position) 
(euler-angles body) orientation (cg-position body) position) 
(transform-node-list body)) 

(defmethod get-delta-t ((body rigid-body) read-clock-flag default-value) 

(if read-clock-flag 

(let* ({old-time (clock-tick-count body)) 

(new-time (setf (clock-tick-count body) (get-internal-real-time)))) 
(/ {- new-time old-time) 1000)) 
default-value)) 

(defmethod update-rigid-body {(body rigid-body) real-time-flag default-dt) 

(let* ((delta-t (get-delta-t body real-time-flag default-dt)) 

(time (time-stamp body) ) 

(pqr (body-coord-angular-velocity body)) 

(uvw (body-coord-linear-velocity body)) 

(x (append (cg-position body) (euler-angles body) uvw pqr)) 

(new-x (heun-step x time delta-t) )) 

(store-state body new-x) 

(setf (H-matrix body) 

(homogeneous-transform (euler-angles body) (cg-position body))) 
(transform-node-list body) 

(setf (time-stamp body) (+ time delta-t)))) 

{defun derivative (state time) 

(let* ((fm (forces-moments state time)) 

(f (first-three fm)) 

(fm (nthcdr 3 fm)) 

(moments (first-three fm) ) 

;(f (forces state time)) (moments (moments state time)) 

(location (first-three state)) (state (nthcdr 3 state)) 

(angles (first-three state)) (state (nthcdr 3 state)) 

(uvw (first-three state)) (state (nthcdr 3 state)) (pqr state) 

(R (rotation-matrix angles)) (v-earth (post-multiply R uvw)) 

(M (body-rate-to-euler-rate-matrix angles)) 

(euler-angle-rates (post-multiply M pqr)) 

(g-body (post-multiply (transpose R) *gravity*)) 

(negative-pqr (scalar-multiply -1 pqr)) 

(f/m (scalar-multiply *inverse-mass* f)) 

(coreolis (cross-product negative-pqr v-earth)) 

(uvw-dot (vector-add f/m (vector-add g-body coreolis))) 

(H (post-multiply *1* pqr)) 

(pqr-dot (post-multiply *I-inv* (vector-subtract moments (cross-product 
pqr H))))) 

(setf (output-vector-to-linkl (foot human-1)) (append f/m pqr-dot euler- 
angle-rates angles)) 

(append v-earth euler-angle-rates uvw-dot pqr-dot))) 

(defmethod store-state ((body rigid-body) state) 

(let* ((location (first-three state)) (state (nthcdr 3 state)) 

(angles (first-three state)) (state (nthcdr 3 state)) 

(uvw (first-three state)) (state (nthcdr 3 state)) (pqr state)) 

(setf (euler-angles body) angles (cg-position body) location 
(body-coord-angular-velocity body) pqr 
(body-coord-linear-velocity body) uvw))) 
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(defun first-three (list) (list (first list) (second list) (third list))) 

(defmethod transform-node-list ((body rigid-body)) 

(setf (transformed-node-list body) 

(mapcar #'(lambda (node-location) 

(post-multiply (H-matrix body) node-location)) 

(node-list body)))) 

(defconstant *gravity* '(0 0 32.2185)) 

;(defun forces (state time) (scalar-multiply -1 *gravity*)) 

(defun moments (state time) '(0 0 0)) 

(defun test () 

(setf airplane-1 (make-instance 'rigid-body)) 

(setf camera-1 (make-instance 'strobe-camera)) 

(move camera-1 (list 0 (- (/ pi 2)) 0) '(00 -30)) 

(initialize airplane-1) 

(take-picture camera-1 airplane-1) 

(dotimes (i 20 'done) (update-rigid-body airplane-1 nil .1)) 
(take-picture camera-1 airplane-1) 

(time-stamp airplane-l)) 

(def\an repeat () 

(initialize airplane-1) 

(dotimes (i 10 'done) (update-rigid-body airplane-1 nil .1)) 
(take-picture camera-1 airplane-1) 

(time-stamp airplane-1)) 
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**************************************************************************** 
File iPC-camera.cl 

Author :Dr, R, McGhee 

:Naval Postgraduate School 
:Monterey/ CA 93943 

Summary :"Takes picture" of a rigid body 
**************************************************************************** 


(defclass strobe-camera {rigid-body) 

((focal-length 

:accessor focal-length 
linitform 1) 

(camera-window 
:accessor camera-window 
:initform(open-stream 'bitmap-window 
*1isp-main-window* 

:output 

: window-exterior 

(make-box 720 0 1020 300) 

;title "Inverted-Pendulum - Looking West")) 

(H-matrix ;The first vector (psi theta phi) = (roll elevation azimuth). 

:accessor H-matrix ;The second vector (xe ye ze) = (north east down). 
rinitform (homogeneous-transform '(0 0 -1.57) '(030))) 

(inverse-H-matrix 
:accessor inverse-H-matrix 

:initform (inverse-H (homogeneous-transform '(0 0 -1.57) '(0 3 0)))) 

(enlargement-factor 
:accessor enlargement-factor 
cinitform 10))) 

(defmethod move-body ( (camera strobe-camera) orientation position) 

(setf (H-matrix camera) (homogeneous-transform orientation position)) 

(setf (inverse-H-matrix camera) (inverse-H (H-matrix camera)))) 

(defmethod take-picture ( (camera strobe-camera) (body rigid-body)) 

(let ((camera-space-node-list (mapcar #'(lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body)))) 

(dolist (polygon (polygon-list body)) 

(clip-and-draw-polygon camera polygon camera-space-node-1 ist))) ) 

(defclass still-camera (strobe-camera) ()) 

(defmethod take-picture ((camera still-camera) (body rigid-body)) 

(clear-page (camera-window camera)) 

(let ((camera-space-node-list (mapcar #'(lambda (node-location) 
(post-multiply (inverse-H-matrix camera) node-location)) 
(transformed-node-list body)))) 

(dolist (polygon (polygon-list body)) 

(clip-and-draw-polygon camera polygon camera-space-node-list)))) 

(defmethod clip-and-draw-polygon 

((camera strobe-camera) polygon node-coord-list) 

(do* {{initial-point (nth (first polygon) node-coord-list)) 

(from-point initial-point to-point) 

(remaining-nodes (rest polygon) (rest remaining-nodes)) 

(to-point (nth (first remaining-nodes) node-coord-list) 

(if (not (null (first remaining-nodes))) 
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(nth (first remaining-nodes) node-coord-list)))) 

((null to-point) 

(draw-clipped-projection camera from-point initial-point)) 
(draw-clipped-projection camera from-point to-point))) 

(defmethod draw-clipped-projection ((camera strobe-camera) from-point to-point) 
(cond ((and (<= (first from-point) (focal-length camera)) 

(<= (first to-point) (focal-length camera))) nil) 

((<= (first from-point) (focal-length camera)) 
(draw-line-in-camera-window camera 

(perspective-transform camera (from-clip camera from-point to-point)) 
(perspective-transform camera to-point))) 

((<= (first to-point) (focal-length camera)) 

(draw-line-in-camera-window camera 

(perspective-transform camera from-point) 

(perspective-transform camera (to-clip camera from-point to-point)))) 
(t (draw-line-in-camera-window camera 

(perspective-transform camera from-point) 

(perspective-transform camera to-point))))) 

(defmethod from-clip ((camera strobe-camera) from-point to-point) 

(let ((scale-factor (/ {- (focal-length camera) (first from-point)) 

(- (first to-point) (first from-point))))) 

(list (+ (first from-point) 

(* scale-factor (- (first to-point) (first from-point)))) 

(+ (second from-point) 

(* scale-factor (- (second to-point) (second from-point)))) 

{+ (third from-point) 

(* scale-factor (- (third to-point) (third from-point)))) 1))) 

(defmethod to-clip ((camera strobe-camera) from-point to-point) 

(from-clip camera to-point from-point)) 

(defmethod draw-line-in-camera-window ((camera strobe-camera) start end) 
(draw-line (camera-window camera) 

(make-position (first start) (second start)) 

(make-position (first end) (second end)))) 

(defmethod perspective-transform ((camera strobe-camera) point-in-camera-space) 
(let* ((enlargement-factor (enlargement-factor camera)) 

(focal-length (focal-length camera)) 

(X (first point-in-camera-space)) ;x axis is along optical axis 
(y (second point-in-camera-space)) ;y is out right side of camera 
(z (third point-in-camera-space))) ;z is out bottom of camera 
(list (+ (round (* enlargement-factor (/ (* focal-lengthy) x))) 

150) ;to right in camera window 

(+ 150 (round (* enlargement-factor (/ (* focal-length z) x)) 

))))) ;up in camera window 
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*************'*-************************************************* 
link.cl 
Dr. R. McGhee 
Naval Postgraduate School 
Monterey, CA 93943 
David A. Bret 2 

Defines the classes of a rigid body link, rotary link, and 
inverted pendulum link 


f 


(defclass link (rigid-body) 

{(motion-limit-flag 
rinitform nil 

:accessor motion-limit-flag) 
(twist-angle 
: initarg : twist-eingle 
:accessor twist-angle) 

(link-length 
:initarg :1ink-length 
:accessor link-length) 

(inboard-j oint-angle 
:initarg :inboard-joint-angle 
:accessor inboard-joint-angle) 

(inboard-j oint-displacement 
:initarg :inboard-joint-displacement 
:accessor inboard-j oint-displacement) 
(inboard-1ink 
:initarg :inboard-link 
:accessor inboard-link) 

(A-matrix 

:accessor A-matrix))) 


(defclass rotary-link (link) 

( (min-joint-angle 

:initarg :min-joint-angle 
:accessor min-joint-angle) 

(max- j o int - angl e 
:initarg :max-joint-angle 
:accessor max-joint-angle))) 


(defclass inverted-pendul^Im-link (rotary-link) 

((outboard-link 

:initarg :outboard-1ink 
:accessor outboard-link) 

(body-coord-angular-acceleration ;The vector (p-dot q-dot r-dot) 
:initform MO 0 0) 

:initarg body-coord-angular-acceleration 
:accessor body-coord-angular-acceleration) 

(posture-translation-rate ;The vector (xe-dot ye-dot ze-dot). 

linitform (list 000) 

:initarg :posture-translation-rate 
:accessor posture-translation-rate) 

(posture-translation-acceleration ;The vector (xe-double-dot 

; ye-double-dot ze-double-dot). 

linitform (list 000) 

:initarg :posture-translation-acceleration 
:accessor posture-translation-acceleration) 

(body-forces ;The vector (Fx Fy Fz) in body coordinates. 
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rinitform (list 000) 
rinitarg :body-forces 
:accessor body-forces) 

(body-torques ;The vector (L M N) in body coordinates, 
rinitform (list 000) 

:initarg :body-torques 
:accessor body-torques) 

(equilibrium-torque ;The torque required to maintain 

; zero rotational acceleration. 

: init form 0 

;initarg :equilibrium-torque 
:accessor equilibrium-torque) 

(unit-acceleration-torque 7 The torque required to maintain 

; unity rotational acceleration. 

: init form 0 

:initarg :unit-acceleration-torque 
:accessor unit-acceleration-torque) 

(C-col\imn-vector 
:initform 0 

:initarg :C-column-vector 
:accessor C-column-vector) 

(equilibrium-inboard-joint-angle 
rinitform 0 

:initarg :equilibrium-inboard-j oint-angle 
:accessor equilibrium-inboard-joint-angle) 

(old-inboard-j oint-angle 
:initform 0 

rinitarg :old-inboard-joint-angle 
:accessor old-inboard-joint-angle))) 
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File 

Author 


Modified by 
Summary 


: human-1 ink. c 1 
: Dr. R. McGhee 
:Naval Postgraduate School 
:Monterey, CA 93943 
:David A. Bretz 

:Defines foot and individual link classes 
:Also includes code that rotates rotary links 




(defclass foot (rigid-body) 

{(node-list ; (x y z 1) in body coord for each node. Starts with (0 0 0 1). 
:initform '((0001) (0.19047619 0 -0.2285714 1) (-0.495238 0 0.1142857 1) 

(0.3047619 0.0761905 0.1142857 1) (0.3047619 -0.0761905 

0.1142857 1))) 

(polygon-list ;initform '((123) (134) (24))) 

; (motion-limit-flag 

; : initform nil 

; :accessor motion-limit-flag) 

(H-matrix rinitform (homogeneous-transform '(000) '(000))) 

(mass rinitform 0.08112) 

(output-vector-to-linkl 
rinitfoorm '(000000000000) 
rinitarg :output-vector-to-linkl 
:accessor output-vector-to-linkl) 

(ground-contac t-flag 
rinitform nil 

raccessor ground-contact-flag) 

(old-euler-angles 
rinitform '(000) 
rinitarg rold-euler-angles 
raccessor old-euler-angles) 

)) 

(defclass linkO (inverted-pendulum-link) 

((twist-angle rinitform (deg-to-rad -90)) 

(link-length rinitform 0.19047619) 

(inboard-joint-angle rinitform (deg-to-rad 0)) 

(inboard-joint-displacement rinitform -0.2285714) 

(min-joint-angle rinitform (deg-to-rad -360)) 

(max-joint-angle rinitform (deg-to-rad 360)) 

(node-list rinitform '{(0001) (0001) (-0.19047619 -0.2285714 01))) 

(polygon-list rinitform '((12))) 

(moment-of-inertia-matrix rinitform '((1 0 0) (0 1 0) (0 0 1))))) 

(defclass linkl (inverted-pendulum-link) 

({twist-angle rinitfoim 0) 

(link-length rinitform 1.55) 

(inboard-joint-angle rinitform (deg-to-rad 90)) 

{inboard-j oint-displacement r initform 0) 

(min-joint-angle rinitform (deg-to-rad 30)) 

(max-joint-angle rinitform (deg-to-rad 150)) 

(node-list rinitform '{(0001) (0 0 0 1) (-1.55 0 0 1))) 

(polygon-list rinitform '((1 2))) 

(mass rinitform 0.25176) 

(moment-of-inertia-matrix rinitform '{(1 0 0) (0 0.0372 0) (0 0 1))) 

(output-f-m-to-foot 
rinitform '(0 0 0) 


59 




:initarg :output-f-m-to-foot 
caccessor output-f-m-to-foot) 

)) 

(defclass 1ink2 {inverted-pendulum-1ink) 

{(twist-angle linitform 0) 

(link-length :initform 1.447) 

(inboard-joint-angle :initform (deg-to-rad -1)) 

(inboard-j oint-displacement : initform 0) 

(min-joint-angle :initform (deg-to-rad -90)) 

(max-joint-angle rinitform (deg-to-rad 2)) 

(node-list rinitform '{(0001) (0001) (-1.447 001))) 

(polygon-list rinitform '((12))) 

(mass rinitform 0.5874) 

(moment-of-inertia-matrix rinitform '((100) (0 0.0776 0) (001))))) 

(defclass links (inverted-pendulum-link) 

{{twist-angle rinitform 0) 

(link-length rinitform 2.24) 

(inboard-joint-angle rinitform (deg-to-rad 1)) 

(inboard-joint-displacement rinitform 0) 

(min-joint-angle rinitform (deg-to-rad -40)) 

(max-joint-angle rinitform (deg-to-rad 90)) 

(node-list rinitform '((0001) (0001) (-2.24 001))) 

(polygon-list rinitform '((1 2) ) ) 

(mass rinitform 3.754) 

(moment-of-inertia-matrix rinitform '((1 0 0) (0 1.0 0) (0 0 1))))) 

(defmethod update-A-matrix ((link link)) 

(with-slots (twist-angle link-length inboard-joint-angle 
inboard-joint-displacement A-matrix) link 
(setf A-matrix (dh-matrix inboard-joint-angle 

twist-angle 
link-length 

inboard-joint-displacement)))) 

• (defmethod rotate ((link link) angle) 

(setf (inboard-joint-angle link) angle) 

(update-A-matrix link) 

(setf (H-matrix link) (matrix-multiply (H-matrix (inboard-link link)) 

(A-matrix link))) 

(transform-node-list link)) 

(defmethod rotate-link ((link link) angle) 

(cond ( {> ^gle (max-joint-angle link)) 

(rotate link (max-joint-angle link)) 

(setf (motion-limit-flag link) t)) 

((< angle (min-joint-angle link)) 

(rotate link (min-joint-angle link)) 

(setf {motion-limit-flag link) t)) 

(t (rotate link angle) (setf {motion-limit-flag link) nil)))) 
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**********************************'*******'**‘*******'*********>***************** 
File :h\aman-model. cl 

Author :David A. Bretz 

:Naval Postgraduate School 
:Monterey, CA 93943 

Sxunmary :defines the four-link human used for testing in this thesis 
**********★★**■*******************■******'*****************■*****★********■****** 

(defclass four-link-hiiman {) 

{(foot 

:initform (make-instance 'foot) 

:accessor foot) 

(linkO 

: initform (make-instance 'linkO) 

:accessor linkO) 

(linkl 

:initform (make-instance 'linkl) 

:accessor linkl) 

(link2 

linitform (make-instance 'link2) 

:accessor link2) 

(link3 

rinitform (make-instance 'link3) 

:accessor link3) 

(motion-complete-flag 
rinitform nil 

:accessor motion-complete-flag) 

(previous-foot-position 
rinitform nil 

raccessor previous-foot-position) 

(current-foot-position 
rinitform nil 

raccessor current-foot-position) 

(commanded-horizontal-angle-list 
rinitform '(1.7453 1.3963 1.7453) 
rinitarg r commanded-horizontal-angle-list 
:accessor commanded-horizontal-angle-list) 

(equi 1 ibrium- tor que-vector 
rinitform '(00 0) 

rinitarg r equilibrium-torque-vector 
raccessor equilibrium-torque-vector) 

(unit-acceleration-torque-vector 
rinitform '(0 0 0) 

rinitarg runit-acceleration-torque-vector 
r accessor unit-acceleration-torque-vector) 

(C-matrix 

rinitform '((000)(000)(000)) 
rinitarg r C-matrix 
:accessor C-matrix) 

(number-of-1inks 
rinitform 3 

raccessor number-of-links) 

(link-list 

rinitform '(linkl link2 link3) 
raccessor link-list) 

(horizontal-angle-vector 
rinitform '(1.7453 1.3963 1.7453) 
rinitarg rhorizontal-angle-vector 
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;accessor horizontal-angle-vector) 

{angular-velocity-vector 
:initform '(0 0 0) 

:initarg : ANGULAR-VELOCITY-VECTOR 
: accessor ANGULAR-VELOCITY-VECTOR) 

(k-theta 

linitform '(-500 -100 -35) 

:accessor k-theta) 

(k-theta-dot 

:initfonn *(-500 -100 -30) 

:accessor k-theta-dot))) 

(defmethod initialize-human ((human four-link-hxman)) 

(transform-node-list (foot human)) 

(setf (inboard-link (linkO human)) (foot human)) 

(setf (inboard-link (linkl human)) (linkO human)) 

(setf (inboard-link (link2 human)) (linkl hiaman)) 

(setf (inboard-link (link3 human)) (link2 hiaman)) 

(rotate-link (linkO human) (inboard-joint-angle (linkO human))) 
(rotate-link (linkl human) (inboard-joint-angle (linkl human))) 
(rotate-link (link2 htiman) (inboard-joint-angle {link2 human))) 
(rotate-link (link3 human) (inboard-joint-angle (link3 human))) 

(setf (outboard-link (linkO hiiman) ) (linkl human)) 

(setf (outboard-link (linkl human)) (link2 human)) 

(setf (outboard-link (link2 human)) (link3 human)) 

(setf (outboard-link (links human)) (linkO human)) ;set to linkO only for 
forces calculation 

(setf (commanded-horizontal-angle-list human) 

(list (inboard-joint-angle (linkl human)) 

(+ (inboard-joint-angle (linkl human)) 

(inboard-joint-angle (link2 human))) 

^ (+ (inboard-joint-angle (linkl hiiman)) 

(inboard-joint-angle {link2 human)) 

(inboard-joint-angle (links human))))) 

(setf {horizontal-angle-vector human) 

(list (inboard-joint-angle (linkl human)) 

(+ (inboard-joint-angle (linkl human)) 

(inboard-joint-angle (link2 h-uman))) 

(+ (inboard-joint-angle (linkl human)) 

(inboard-joint-angle (link2 human)) 

(inboard-joint-angle (links human))))) 

(setf (current-foot-position human) 

(firstn 3 (first (transformed-node-list (links hviman) )))) ) 

(defun human-picture () 

(setf human-1 (make-instance 'four-link-human)) 

(initialize-human human-1) 

. ********** 

I 

(setf camera-1 (make-instance 'strobe-camera)) 

(camera-window camera-1) 

(setf (enlargement-factor camera-1) 100) 

(take-picture camera-1 human-1)) 

. ************ 
t 

(setf camera-2 (make-instance 'strobe-camera)) 

(camera-window camera-2) 

(setf (title (camera-window camera-2)) "Human - Frontal Plane - Looking 
North") 
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(setf (enlargement-factor camera-2) 15) 

(move-body camera-2 '(0 0.2 0) '(-90 0 0)) 

(take-picture camera-2 hximan-1) 

. ************* 

/ 

(setf camera-3 (make-instance 'strobe-camera)) 

(camera-window camera-2) 

(setf (title (camera-window camera-3)) "Human - Transverse Plane - Looking 
Down") 

(setf (enlargement-factor camera-3) 30) 

(move-body camera-3 '(0 -1.57 0) '(00 -90)) 

(take-picture camera-3 human-l)) 

(defun new-picture () 

(take-picture camera-1 human-1)) 

(take-picture camera-2 human-1) 

(take-picture camera-3 human-1)) 


(defconstant null-move-list '((000000) (000))) 

(defmethod take-picture ((camera strobe-camera) (human four-link-human)) 
(take-picture camera (foot h\iman) ) 

(take-picture camera (linkO human)) 

(take-picture camera (linkl hiaman) ) 

(take-picture camera (link2 hiiman) ) 

(take-picture camera (link3 human))) 

(defmethod move-incremental ( (human four-link-human) increment-list) 

(rotate-link (linkO human) 0) 

(rotate-link (linkl human) 

(+ (first increment-list) (inboard-joint-angle (linkl human)))) 
(rotate-link (link2 human) 

(+ (second increment-list) (inboard-joint-angle (link2 human)))) 
(rotate-link (link3 human) 

(+ (third increment-list) (inboard-joint-angle (link3 human)))) 
(setf (previous-foot-position human) (current-foot-position human)) 

(setf (current-foot-position human) 

(firstn 3 (first (transformed-node-list (link3 human))))) 

(setf (motion-complete-flag h\iman) (not (or (motion-limit-flag (linkl human)) 

(motion-limit-flag (link2 human)) 
(motion-limit-flag (link3 

human) ) ))) 

(setf (horizontal-angle-vector human) 

(list (inboard-joint-angle (linkl human)) 

(+ (inboard-joint-angle (linkl human)) 

(inboard-joint-angle (link2 human))) 

(+ (inboard-joint-angle (linkl hiiman)) 

(inboard-joint-angle (link2 human) ) 

(inboard-joint-angle (link3 human)))))) 

(defmethod feasible-movep ( (human four-link-human) allowable-sinkage allowable- 

slippage) 

(and (<= (third (current-foot-position hxoman) ) allowable-sinkage) 

(or (minusp (third (current-foot-position human))) 

(minusp (third (previous-foot-position human))) 

(<= (vector-length (vector-slippage human)) allowable-slippage)))) 
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(defmethod vector-slippage ((hvunan four-link-huitian)) 

(vector-subtract (rest (reverse (previous-foot-position human))) 
(rest (reverse (current-foot-position human))))) 
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*★**’********★***★’******★★*’************************★*********★*************** 
File : human-dynamics . cl 

Author :David A. Bretz 

:Naval Postgraduate School 
:Monterey, CA 93943 

Summary :Contains recursive Newton-Euler inverse and direct dynamics 
: algorithm. 

★ ***********'***********-****‘*‘***************** ★★***★******★**•**■***★*■*★*★★**★*■ 

(defmethod update-whole-hiiman ( (human four-link-human) delta-time) 

(setf (old-euler-angles (foot human)) (euler-angles (foot human))) 
(update-rigid-body (foot human) nil delta-time) 

(let* ((theta-double-dot (get-angular-acceleration human)) 

(theta-dot (angular-velocity-vector human)) 

(theta (horizontal-angle-vector human)) 

(delta-theta-dot (scalar-multiply delta-time theta-double-dot)) 

(delta-theta (vector-add (scalar-multiply delta-time 

(vector-add theta-dot delta- 

theta-dot) ) 

(scalar-multiply (* 0.5 (sqr delta-time)) 
theta-double-dot))) 

(delta-euler (- (second (old-euler-angles (foot human))) 

(second (euler-angles (foot human))))) 

(move-list (list (- (first delta-theta) delta-euler) 

(second delta-theta) 

(third delta-theta))) 

) 

(setf (angular-velocity-vector human) (vector-add theta-dot delta-theta- 
dot) ) 

;(setf (horizontal-angle-vector human) 

; (vector-add delta-theta (horizontal-angle-vector human))) 

(move-incremental human move-list) 

; (dear-page (earnera-window camera-1)) 

(new-pictur e) 

)) 


(defmethod move-away-from-equilibri\im ((human four-link-human) offset-list) 
(let* ((move-list offset-list)) 

(move-incremental human move-list) 

(new-picture))) 

(defmethod stand-up ( (human four-link-human) cycles time-step) 

(dotimes (i cycles 'done) 

(update -my-human human-1 t ime- s tep) 

)) 


(defmethod update-my-human ( (human four-link-human) delta-time) 

(let* ((theta-double-dot (get-angular-acceleration hximan)) 

(theta-dot (angular-velocity-vector hioman)) 

(theta (horizontal-angle-vector h\jman)) 

(delta-theta-dot (scalar-multiply delta-time theta-double-dot)) 

(delta-theta (vector-add (scalar-multiply delta-time (vector-add theta- 
dot delta-theta-dot)) 


(scalar-multiply (* 0.5 (sqr delta-time)) 


theta-double-dot)) ) 


(move-list delta-theta) 
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(time (time-stamp (linkl human)))) 

;(format t "-A -%" theta-double-dot) 

(setf (angular-velocity-vector h\iman) (vector-add theta-dot delta-theta- 
dot) ) 

;(setf (horizontal-angle-vector hviman) (vector-add delta-theta (horizontal- 
angle- vector human))) 

(move-incremental human move-list) 

(setf (time-stamp (linkl human)) (+ time delta-time)) 

(plot plotter-2 time (* 100 (- (first (horizontal-angle-vector human)) 

(first (commanded-horizontal-angle-list 

human))))) 

(plot plotter-3 time (* 100 (- (second (horizontal-angle-vector human)) 

(second (commanded-horizontal-angle-list 

human)) 

))) 

(plot plotter-4 time (* 100 (- (third (horizontal-angle-vector human)) 

(third (commanded-horizontal-angle-list 

human)) 

))) 

; (dear-page (camera-window camera-1)) 

(new-picture) 

)) 

(defmethod get-angular-acceleration ((human four-link-human)) 

(post-multiply (matrix-inverse (transpose (build-c-matrix human))) 
(vector-subtract (get-applied-torque-vector hximan) 

(get-hiaman-torque-vector human '(0 0 0))))) 

(defmethod get-applied-torque-vector ((human four-link-human)) 

(let* ((theta (horizontal-angle-vector human)) 

(theta-goal (commanded-horizontal-angle-list human)) 

(theta-dot (angular-velocity-vector human)) 

(k-theta-i (k-theta human)) 

(k-theta-dot-i (k-theta-dot hiiman) )) 

(vector-add (mapcar '* k-theta-i (vector-subtract theta theta-goal)) 

(mapcar '* k-theta-dot-i (vector-subtract theta-dot 

MO 0 0)))))) 

(defmethod build-c-matrix ((human four-link-human)) 

(let* ((TO (get-h\mian-torque-vector h\mian MO 0 0))) 

(Tl (get-human-torque-vector human Ml 0 0))) 

(T2 (get-h\jman-torque-vector human MO 1 0))) 

(T3 (get-human-torque-vector human '(001)))) 

;(format t "RUN -A -A -% -A -A -%" TO Tl T2 T3) 

(list (vector-subtract Tl TO) 

(vector-subtract T2 TO) 

• (vector-subtract T3 TO)))) 

(defmethod get-h\aman-torque-vector ((human four-link-human) angular- 
accelerations ) 

(let* ((foot-data (output-vector-to-linkl (foot human))) 

(foot-earth-acc (first-three foot-data)) (foot-data (nthcdr 3 foot- 

data) ) 

(foot-angular-acc (first-three foot-data)) (foot-data (nthcdr 3 foot- 

data) ) 
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(foot-angular-velocity (first-three foot-data)) (foot-data (nthcdr 3 
foot-data)) 

(foot-angle (first-three foot-data)) 

(f (get-hnman-force-vector hiiman angular-accelerations)) 

(fx (first f)) 

(fz (second f)) 

(theta-do\ible-dot-0 (second foot-angular-acc)) 

(theta-double-dot-1 (first angular-accelerations)) 

(theta-double-dot-2 (second angular-accelerations)) 

(theta-double-dot-3 (third angular-accelerations)) 

(theta-0 (second foot-angle)) 

(theta-1 (first (horizontal-angle-vector human))) 

(theta-2 (second (horizontal-angle-vector human))) 

(theta-3 (third (horizontal-angle-vector human))) 

(fx4 0) 

(fz4 0) 

(lt4 0) 

(data (list lt4 fx4 theta-3 fz4 theta-double-dot-3 (third fx) (third 

fz))) 

(lt3 (get-link-torque (link3 human) data)) 

(data (list lt3 (third fx) theta-2 (third fz) theta-double-dot-2 
(second fx) (second fz))) 

(lt2 (get-link-torque (link2 hximan) data)) 

'(data (list lt2 (second fx) theta-1 (second fz) theta-double-dot-1 
(first fx) (first fz))) 

(Itl (get-link-torque (linkl.human) data))) 

(setf (output-f-m-to-foot (linkl human)) (list (first fx) (first fz) Itl)) 
(list Itl lt2 lt3))) 

(defmethod get-link-torque ((pendulum inverted-pendulum-link) data) 

(let* ((T-i+1 (first data)) 

(Fx-i+1 (second data)) 

(theta-i (third data)) 

(Fz-i+1 (fourth data)) 

(length (* 0.5 (link-length pendulum))) 

(J-i (second (second (moment-of-inertia-matrix pendulum)))) 
(theta-double-dot-i (fifth data)) 

(Fx-i (sixth data)) 

(fz-i (seventh data))) 

(+ T-i+1 (* Fx-i+1 length (sin theta-i) -1) (* Fz-i+1 length (cos theta-i)) 

(* J-i theta-double-dot-i) (* Fx-i length (sin theta-i) -1) (* Fz-i 

length (cos theta-i))))) 

(defmethod get-human-force-vector ((hiaman four-link-human) angular- 
accelerations) 

(let* ((linear-accelerations (get-human-linear-accelerations human angular- 
accelerations) ) 

(x-double-dot (first linear-accelerations)) 

(z-double-dot (second linear-accelerations)) 

(mass-0 (mass (foot human))) 

(mass-1 (mass (linkl human))) 

(mass-2 (mass (link2 human))) 

(mass-3 (mass (link3 human))) 

(fx4-fz4 MO 0)) 

(data (list fx4-fz4 mass-3 (third x-double-dot) (third z-double- 

dot) )) 

(fx3-fz3 (get-link-forces (link3 human) data)) 
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(data (list fx3-f23 mass-2 (second x-double-dot) (second z-double- 

dot))) 

(fx2-fz2 (get-link-forces (link2 human) data)) 

(data (list fx2-fz2 mass-1 (first x-double-dot) (first z-double- 

dot) ) ) 

(fxl-fzl (get-link-forces (linkl human) data)) 

) 

(list (list (first fxl-fzl) (first fx2-fz2) (first fx3-fz3)) 

(list (second fxl-fzl) (second fx2-fz2) (second fx3-fz3))) 

) 

) 

(defmethod get-link-forces ((pendulum inverted-pendul\im-link) data) 

(let* ((fx-i+1 (first (first data))) 

(fz-i+l (second (first data))) 

(mass (second data)) 

(x-double-dot-i (third data)) 

(z-double-dot-i (third data))) 

(list (+ fx-i+1 (* mass x-double-dot-i)) 

{+ fz-i+l (* mass z-double-dot-i) (* mass (third *gravity*) -1))))) 


(defmethod get-hxaman-linear-accelerations ( (human four-link-h\iman) angular- 
accelerations) 

(let* ((foot-data (output-vector-to-linkl (foot human))) 

(foot-earth-acc (first-three foot-data)) (foot-data (nthcdr 3 foot- 

data )) 


(foot-angular-acc (first-three foot-data)) (foot-data (nthcdr 3 foot- 

data) ) 


(foot-angular-velocity (first-three foot-data)) (foot-data (nthcdr 3 
foot-data)) 

(foot-angle (first-three foot-data)) 

(theta-double-dot-0 (second foot-angular-acc)) 

(theta-double-dot-1 (first angular-accelerations)) 

(theta-double-dot-2 (second angular-accelerations)) 

(theta-double-dot-3 (third angular-accelerations)) 

(theta-dot-0 (second foot-angular-velocity)) 

(theta-dot-1 (first (angular-velocity-vector human))) 

(theta-dot-2 (second (angular-velocity-vector human))) 

(theta-dot-3 (third (angular-velocity-vector human))) 

(theta-0 (second foot-angle)) 

(theta-1 (first (horizontal-angle-vector human))) 

(theta-2 (second (horizontal-angle-vector human))) 

(theta-3 (third {horizontal-angle-vector human))) 

(xO-zO (list (first foot-earth-acc) (third foot-earth-acc))) 

(data (list xO-zO theta-0 theta-1 theta-dot-0 theta-dot-1 theta-double 
dot-0 theta-double-dot-1)) 

(xl-zl {link-linear-accelerations (linkl human) data)) 


(data (list xl-zl theta-1 theta-2 
dot-1 theta-double-dot-2)) 

(x2-z2 (link-linear-accelerations 


theta-dot-1 theta-dot-2 
{link2 human) data)) 


theta-double 


(data (list x2-z2 theta-2 theta-3 theta-dot-2 theta-dot-3 theta-double 
dot-2 theta-double-dot-3)) 

(x3-z3 (link-linear-accelerations (link3 human) data)) 

) 

(list (list (first xl-zl) (first x2-z2) (first x3-z3)) 
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(list (second xl-zl) (second x2-22) (second x3-z3))) 


(defmethod link-linear-accelerations ( (pendulum inverted-pendul\im-link) data) 
(let* ((x-double-dot-i-1 (first (first data))) 

(z-double-dot-i-1 (second (first data))) 

(theta-i-1 (second data)) 

(theta-i (third data)) 

(theta-dot-i-1 (fourth data)) 

(theta-dot-i (fifth data)) 

(theta-double-dot-i-1 (sixth data)) 

(theta-double-dot-i (seventh data))) 

(list (- x-double-dot-i-1 

(* (* (link-length (inboard-1 ink penduliun)) 0.5) 

(+ (* theta-double-dot-i-1 (sin theta-i-1)) 

(* (sqr theta-dot-i-1) (cos theta-i-1)))) 

(* (* (link-length pendulum) 0.5) 

(+ (* theta-double-dot-i (sin theta-i)) 

(* (sqr theta-dot-i) (cos theta-i))))) 

(~ (* z-double-dot-i-1 -1) 

(* (* (link-length (inboard-link pendulum)) 0.5) 

(+ (* theta-double-dot-i-1 (cos theta-i-1)) 

(* (sqr theta-dot-i-1) (sin theta-i-1)))) 

(* (* (link-length pendulum) 0.5) 

(+ (* theta-double-dot-i (cos theta-i)) 

(* (sqr theta-dot-i) (sin theta-i)))))))) 

(defiin sqr (x) 

(* X X)) 
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. **************************************************************************** 
; File :foot-dynamics.cl 

; Author :David A. Bretz 

; :Naval Postgraduate School 

; :Monterey, CA 93943 

; Summary ;Calculates the forces and moments acting on the foot. 

; :Called from the euler-angle-rigid-body code. 

; :Also defines the ground class. 

. **************************************************************************** 


(defclass ground (rigid-body) 

((node-list 

linitform '((0001) (0001) (5001) (-5001) (0501) (0-501))) 

(polygon-list :initfonn '((12) (13) (14) (15) (2435))) 

)) 

(defim forces-moments (state time) 

(let* ((z (third state)) 

(theta (fifth state)) 


(location (first-three state)) (state (nthcdr 3 state)) 

(angles (first-three state)) (state (nthcdr 3 state)) 

(uvw (first-three state)) (state (nthcdr 3 state)) (pqr state) 

(R (rotation-matrix angles)) (v-earth (post-multiply R uvw)) 

(M (body-rate-to-euler-rate-matrix angles)) 

(euler-angle-rates (post-multiply M pqr)) 

(fxl (first (output-f-m-to-foot (linlcl human-1)))) 

(fzl (second (output-f-m-to-foot (linkl human-1)))) 

(tl (third (output-f-m-to-foot (linkl human-1)))) 

(u (first uvw)) 

(w (third uvw)) 

(xe-dot (first v-earth)) 

(ze-dot (third v-earth)) 

(theta-dot (second euler-angle-rates)) 

(torsional-damening-const 0) 

(spring-const 10000) 

(darapening-const 100) 

(ze-toe (+ z (* 0.508253869 (sin (+ theta 0.226798848054))))) 

(ze-heel (+ z (* 0.325485857 (sin (- 0.358770670271 theta))))) 
(fze-toe (cond ((>= ze-toe 0) (+ (* spring-const ze-toe -1) 

(* dampening-const ze-dot -1))) 

({< ze-toe 0) 0))) 

(fze-heel (cond ((>= ze-heel 0) (+ (* spring-const ze-heel -1) 

(* daii®)ening-const ze-dot -1))) 

((< ze-heel 0) 0))) 

(fx-body (+ (* fze-toe (sin theta) -1) 

(* fze-heel (sin theta)) 

;(* dampening-const xe-dot 0) 

(* fxl (sin theta) -1))) 

(fz-body (+ (* fze-toe (cos theta)) 

(* fze-heel (cos theta)) 

(* fzl (cos theta) -1))) 

(m-body (+ (* fx-body 0.1142857); includes fxl but at wrong distance! 
(* fze-toe (cos theta) 0.495238) 
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(* fze-heel (sin theta) 0.3047619); cos 
(* torsional-damening-const theta-dot -1) 
(* tl -1))) 

(fz-ground (+ (* fze-toe (cos theta)) 

(* fze-heel (cos theta)))) 


) 

;(format t "state -A ~%" (append location angles uvw pqr)) 

(format t "fz-ground -A -%" fz-ground) 

(format t "forces -A -A -A -A -A -%" fze-toe fze-heel fx-body fz-body m- 
body) 

(setf (ground-contact-flag (foot h\iman-l)) (cond ((or {>= ze-toe 0) (>= ze- 

heel 0)) t))) 

(plot plotter-1 time (- fz-ground)) 

(list fx-body 0 fz-body 0 m-body 0))) 
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**************************************************************************** 
File :test.cl 

Author :David A. Bretz 

:Naval Postgraduate School 
:Monterey, CA 93943 
Summary :top-level functions for 
:1)testing the foot alone 
:2)testing the postural control 
:3)testing the four-link-human 

**************************************************************************** 


(defun test-foot () 

(setf foot-1 (make-instance 'foot)) 

(setf ground-1 (make-instance 'ground)) 

(setf camera-1 (make-instance 'strobe-camera)) 

(setf plotter-1 (make-instance 'plotter)) 

(draw-coordinate-axes (plotter-window plotter-1)) 

(initialize foot-1) 

(initialize ground-1) 

(move foot-1 '(000) '(00 -0.121)) 

(setf (enlargement-factor camera-1) 100) 

(take-picture camera-1 foot-1) 

(take-picture camera-1 ground-1) 

(dotimes (i 100 'done) (update-rigid-body foot-1 nil .001)) 

(take-picture camera-1 foot-1) 

(dotimes (i 1 'done) 

(time-stamp foot-1) 

(dotimes (i 100 'done) (update-rigid-body foot-1 nil .001)) 

(take-picture camera-1 foot-1) 

(time-stamp foot-1) 

) 

) 

(defim test-posture () 

(human-picture) 

(setf plotter-2 (make-instance 'plotter)) 

(draw-coordinate-axes (plotter-window plotter-2)) 

(setf plotter-3 (make-instance 'plotter)) 

(draw-coordinate-axes (plotter-window plotter-3)) 

(setf plotter-4 (make-instance 'plotter)) 

(draw-coordinate-axes (plotter-window plotter-4)) 

(move-away-from-equilibrium human-1 '(0 0 0)) ;delta change of vertical theta 
(stand-up human-1 1000 0.001) ;cycles and time-step 
) 


(defun big-test () 

(human-picture) 

(setf ground-1 (make-instance 'ground)) 

(setf plotter-1 (make-instance 'plotter)) 

(draw-coordinate-axes (plotter-window plotter-1)) 

(initialize ground-1) 

(take-picture camera-1 ground-1) 

(move (foot human-1) '(000) '(00 -0.1142857)) 

(move-incremental human-1 '(0 0 0)) 

;(move (foot human-1) '(0 -0.32175 0) '(00 -0.2376)) 

; (move-incremental human-1 '(-0.02702 0 0.4)) 

;(setf (angular-velocity-vector human-1) '(-.973 -.973 -.973)) 

; (setf (body-coord-angular-velocity (linkl human-1)) (list 0 -.973 0) ) 
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; (setf (body-coord-linear“-velocity (linkl human-l)) (list 0 
;(setf (body-coord-angular-velocity (link2 human-l)) (list 0 
;(setf (body-coord-linear-velocity (link2 human-l)) (list -2 
;(setf (body-coord-angular-velocity (linkS human-l)) (list 0 
;(setf (body-coord-linear-velocity (linkS human-l)) (list -4 
(dotimes (i 400 'done) 

(update-whole-human human-l .0001) 

) 

) 


0 0 )) 

-.5 0 )) 
.0 0 0 )) 
0 0 )) 

.5 0 0)) 
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